Beispiel #1
0
    bool
    TimeProfile::parse(const IMC::YoYo* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      Position pos;
      extractPosition(maneuver, pos);

      // Use 2D distance here
      float horz_dist = distance2D(pos, last_pos);
      float travelled = horz_dist / std::cos(maneuver->pitch);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      float duration = travelled / speed;

      // Update speed profile
      m_speed_vec->push_back(SpeedProfile(maneuver, duration));

      m_accum_dur->addDuration(duration);

      return true;
    }
//ASTEROID
Asteroid::Asteroid()
{
	rotateX = 0;
	rotateY = 0;
	rotateZ = 0;

	posX = 0;
	posY = 0;
	posZ = 0;

	sizeX = 0;
	sizeY = 0;
	sizeZ = 0;

	moveSpeed = 0;

	resetStroid = false;

	dLeft = false;
	dRight = false;
	dead = false;

	resetBool = false;
	resetTimer = 0;

	convertSpeed();
	convertWeight();
}
Beispiel #3
0
    bool
    Duration::parse(const IMC::Rows* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      Maneuvers::RowsStages rstages = Maneuvers::RowsStages(maneuver, NULL);

      Position pos;
      pos.z = maneuver->z;
      pos.z_units = maneuver->z_units;

      rstages.getFirstPoint(&pos.lat, &pos.lon);

      float distance = distance3D(pos, last_pos);
      m_accum_dur->addDuration(distance / speed);

      last_pos = pos;

      distance += rstages.getDistance(&last_pos.lat, &last_pos.lon);

      std::vector<float>::const_iterator itr = rstages.getDistancesBegin();

      for (; itr != rstages.getDistancesEnd(); ++itr)
      {
        // compensate with path controller's eta factor
        float travelled = compensate(*itr, speed);
        m_accum_dur->addDuration(travelled / speed);
      }

      return true;
    }
Beispiel #4
0
    float
    Duration::parse(const IMC::Rows* maneuver, Position& last_pos, float last_dur,
                    std::vector<float>& durations, const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      Maneuvers::RowsStages rstages = Maneuvers::RowsStages(maneuver, NULL);

      Position pos;
      pos.z = maneuver->z;
      pos.z_units = maneuver->z_units;

      rstages.getFirstPoint(&pos.lat, &pos.lon);

      float distance = distance3D(pos, last_pos);
      durations.push_back(distance / speed + last_dur);

      last_pos = pos;

      distance += rstages.getDistance(&last_pos.lat, &last_pos.lon);

      std::vector<float>::const_iterator itr = rstages.getDistancesBegin();

      for (; itr != rstages.getDistancesEnd(); ++itr)
      {
        // compensate with path controller's eta factor
        float travelled = compensate(*itr, speed);
        durations.push_back(travelled / speed + durations.back());
      }

      return distance / speed;
    }
void SerialInterface::configure()
{
  //fcntl(m_fd, F_SETFL, FNDELAY);
  fcntl(m_fd, F_SETFL, 0);
  struct termios port_settings;      // structure to store the port settings in
  bzero(&port_settings, sizeof(port_settings));

  speed_t speed = convertSpeed(m_baudRate);

  cfsetispeed(&port_settings, speed);    // set baud rates
  cfsetospeed(&port_settings, speed);

  port_settings.c_cflag &= ~PARENB;    // set no parity, stop bits, data bits
  port_settings.c_cflag &= ~CSTOPB;
  port_settings.c_cflag &= ~CSIZE;
  port_settings.c_cflag |= CS8;

  port_settings.c_cflag &= ~CRTSCTS;

  port_settings.c_cflag |= CREAD | CLOCAL;  // turn on READ & ignore ctrl lines
  port_settings.c_iflag &= ~(IXON | IXOFF | IXANY); // turn off s/w flow ctrl

  port_settings.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw
  port_settings.c_oflag &= ~OPOST; // make raw

  // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
  port_settings.c_cc[VMIN]  = 0;
  port_settings.c_cc[VTIME] = 20;

  if(tcsetattr(m_fd, TCSANOW, &port_settings) < 0)    // apply the settings to the port
  {
    printf("Error: Failed to set serial settings\n");
  }
}
Beispiel #6
0
    float
    Duration::parse(const IMC::YoYo* maneuver, Position& last_pos, float last_dur,
                    std::vector<float>& durations, const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      extractPosition(maneuver, pos);

      // Use 2D distance here
      float horz_dist = distance2D(pos, last_pos);
      float travelled = horz_dist / std::cos(maneuver->pitch);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      durations.push_back(travelled / speed + last_dur);

      return durations.back();
    }
Beispiel #7
0
    float
    Duration::parse(const IMC::Elevator* maneuver, Position& last_pos, float last_dur,
                    std::vector<float>& durations, const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      extractPosition(maneuver, pos);

      float goto_dist = distance3D(pos, last_pos);
      float amplitude = std::fabs(last_pos.z - maneuver->end_z);
      float real_dist = amplitude / std::sin(c_rated_pitch);

      float travelled = goto_dist + real_dist;

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      durations.push_back(travelled / speed + last_dur);

      return durations.back();
    }
Beispiel #8
0
    bool
    TimeProfile::parse(const IMC::Elevator* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      Position pos;
      extractPosition(maneuver, pos);

      float goto_dist = distance3D(pos, last_pos);
      float amplitude = std::fabs(last_pos.z - maneuver->end_z);
      float real_dist = amplitude / std::sin(c_rated_pitch);

      float travelled = goto_dist + real_dist;

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      float duration = travelled / speed;

      // Update speed profile
      m_speed_vec->push_back(SpeedProfile(maneuver, duration));

      m_accum_dur->addDuration(duration);

      return true;
    }
Beispiel #9
0
    bool
    TimeProfile::parse(const IMC::PopUp* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      // Travel time
      float travel_time;

      if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0)
      {
        Position pos;
        extractPosition(maneuver, pos);

        float travelled = distance3D(pos, last_pos);

        // compensate with path controller's eta factor
        travelled = compensate(travelled, speed);

        travel_time = travelled / speed;

        last_pos = pos;
      }
      else
      {
        travel_time = 0;
      }

      // Update speed profile
      m_speed_vec->push_back(SpeedProfile(maneuver, travel_time));

      // Rising time and descending time
      float rising_time;
      float descending_time;
      if (maneuver->z_units == IMC::Z_DEPTH)
      {
        rising_time = (std::fabs(last_pos.z) / std::sin(c_rated_pitch)) / speed;
        descending_time = (std::fabs(maneuver->z) / std::sin(c_rated_pitch)) / speed;
      }
      else // altitude, assume zero
      {
        rising_time = 0.0;
        descending_time = 0.0;
      }

      // surface time
      float surface_time = c_fix_time;

      // Update speed profile
      m_speed_vec->push_back(SpeedProfile(maneuver, rising_time));
      m_speed_vec->push_back(SpeedProfile(0.0, 0, surface_time));
      m_speed_vec->push_back(SpeedProfile(maneuver, descending_time));

      m_accum_dur->addDuration(travel_time + rising_time + surface_time + descending_time);

      return true;
    }
Beispiel #10
0
    bool
    TimeProfile::parse(const IMC::FollowPath* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      Position pos;
      pos.z = maneuver->z;
      pos.z_units = maneuver->z_units;

      IMC::MessageList<IMC::PathPoint>::const_iterator itr = maneuver->points.begin();

      if (!maneuver->points.size())
      {
        // Update speed profile
        m_speed_vec->push_back(SpeedProfile(0.0, 0));

        // Update duration
        m_accum_dur->addDuration(0.0);
      }
      else
      {
        // Iterate point list
        for (; itr != maneuver->points.end(); itr++)
        {
          if ((*itr) == NULL)
            continue;

          pos.lat = maneuver->lat;
          pos.lon = maneuver->lon;
          Coordinates::WGS84::displace((*itr)->x, (*itr)->y, &pos.lat, &pos.lon);

          float travelled = distance3D(pos, last_pos);

          last_pos = pos;

          float duration = compensate(travelled, speed) / speed;

          // Update speed profile
          m_speed_vec->push_back(SpeedProfile(maneuver, duration));

          // compensate with path controller's eta factor
          m_accum_dur->addDuration(duration);
        }
      }

      return true;
    }
Beispiel #11
0
    float
    Duration::parse(const IMC::PopUp* maneuver, Position& last_pos, float last_dur,
                    std::vector<float>& durations, const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      // Travel time
      float travel_time;

      if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0)
      {
        Position pos;
        extractPosition(maneuver, pos);

        float travelled = distance3D(pos, last_pos);

        // compensate with path controller's eta factor
        travelled = compensate(travelled, speed);

        travel_time = travelled / speed;

        last_pos = pos;
      }
      else
      {
        travel_time = 0;
      }

      // Rising time
      float rising_time;
      if (maneuver->z_units == IMC::Z_DEPTH)
        rising_time = std::fabs(last_pos.z) / speed;
      else // altitude, assume zero
        rising_time = 0.0;

      // surface time
      bool wait = (maneuver->flags & IMC::PopUp::FLG_WAIT_AT_SURFACE) != 0;
      float surface_time = wait ? maneuver->duration : c_fix_time;

      durations.push_back(travel_time + rising_time + surface_time + last_dur);

      return durations.back();
    }
Beispiel #12
0
void Asteroid::onTopCollision(Asteroid * astro)
{
	convertSpeed();
	astro->convertSpeed();
	convertWeight();
	astro->convertWeight();

	double topSpd = calcSpeed;
	double botSpd = astro->calcSpeed;
	double topLb = calcWeight;
	double botLb = astro->calcWeight;
	
	if(topSpd < 0 && botSpd < 0)
	{
		botSpd = dbABS(botSpd);
		topSpd = dbABS(topSpd);

	    astro->moveSpeed = -(((topLb + topSpd) -topLb) + botSpd);
	    moveSpeed= (((topLb - topLb) + botSpd) - moveSpeed);

		if(moveSpeed < maxMoveSpeed && astro->moveSpeed < astro->maxMoveSpeed)
		{
			moveSpeed = maxMoveSpeed;
			astro->moveSpeed =astro->maxMoveSpeed;
		}
		
		if(moveSpeed > dbABS(maxMoveSpeed) && astro->moveSpeed > dbABS(astro->maxMoveSpeed))
		{
			moveSpeed = dbABS(maxMoveSpeed);
			astro->moveSpeed =dbABS(astro->maxMoveSpeed);
		}	

		botSpd = -(botSpd);
		topSpd = -(topSpd);
	}


	if(topSpd > 0 && botSpd > 0)
	{
		dbABS(botSpd);
		dbABS(topSpd);
	    astro->moveSpeed = (((topLb + topSpd) -botLb) + botSpd);
		moveSpeed= -(((topLb - topLb) + botSpd) - moveSpeed);//Not sure if correct
	}

}
Beispiel #13
0
    float
    Duration::parse(const IMC::FollowPath* maneuver, Position& last_pos, float last_dur,
                    std::vector<float>& durations, const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      pos.z = maneuver->z;
      pos.z_units = maneuver->z_units;

      IMC::MessageList<IMC::PathPoint>::const_iterator itr = maneuver->points.begin();
      double total_duration = last_dur;

      if (!maneuver->points.size())
      {
        durations.push_back(0.0);
      }
      else
      {
        // Iterate point list
        for (; itr != maneuver->points.end(); itr++)
        {
          if ((*itr) == NULL)
            continue;

          pos.lat = maneuver->lat;
          pos.lon = maneuver->lon;
          Coordinates::WGS84::displace((*itr)->x, (*itr)->y, &pos.lat, &pos.lon);

          float travelled = distance3D(pos, last_pos);

          last_pos = pos;

          // compensate with path controller's eta factor
          total_duration += compensate(travelled, speed) / speed;
          durations.push_back(total_duration);
        }
      }

      return durations.back();
    }
Beispiel #14
0
    float
    Duration::parseSimple(const Type* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      extractPosition(maneuver, pos);
      float travelled = distance3D(pos, last_pos);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      return travelled / speed;
    }
Beispiel #15
0
    float
    Duration::parseSimple(const Type* maneuver, Position& last_pos,
                          float last_dur, std::vector<float>& durations,
                          const SpeedConversion& speed_conv)
    {
      float speed = convertSpeed(maneuver, speed_conv);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      extractPosition(maneuver, pos);
      float travelled = distance3D(pos, last_pos);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      durations.push_back(travelled / speed + last_dur);
      return durations[0];
    }
void SerialInterface::configure()
{
  fcntl(m_fd, F_SETFL, FNDELAY);
  struct termios port_settings;      // structure to store the port settings in
  bzero(&port_settings, sizeof(port_settings));

  speed_t speed = convertSpeed(m_baudRate);

  cfsetispeed(&port_settings, speed);    // set baud rates
  cfsetospeed(&port_settings, speed);

  port_settings.c_cflag |= ( CLOCAL | CREAD |  CS8);                        // Configure the device : 8 bits, no parity, no control
  port_settings.c_iflag |= ( IGNPAR | IGNBRK );

  // see: http://unixwiz.net/techtips/termios-vmin-vtime.html
  port_settings.c_cc[VMIN]  = 0;
  port_settings.c_cc[VTIME] = 0;

  if(tcsetattr(m_fd, TCSANOW, &port_settings) < 0)    // apply the settings to the port
  {
    printf("Error: Failed to set serial settings\n");
  }
}
Beispiel #17
0
    float
    TimeProfile::parseSimple(const Type* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return -1.0;

      Position pos;
      extractPosition(maneuver, pos);
      float travelled = distance3D(pos, last_pos);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      float duration = travelled / speed;

      // Update speed profile
      m_speed_vec->push_back(SpeedProfile(maneuver, duration));

      return duration;
    }
Beispiel #18
0
    bool
    Duration::parse(const IMC::YoYo* maneuver, Position& last_pos)
    {
      float speed = convertSpeed(maneuver);

      if (speed == 0.0)
        return false;

      Position pos;
      extractPosition(maneuver, pos);

      // Use 2D distance here
      float horz_dist = distance2D(pos, last_pos);
      float travelled = horz_dist / std::cos(maneuver->pitch);

      // compensate with path controller's eta factor
      travelled = compensate(travelled, speed);

      last_pos = pos;

      m_accum_dur->addDuration(travelled / speed);

      return true;
    }
Beispiel #19
0
ev3_error_t Ev3BaseCom::drive(drive_t drv) {
	// check for available EV3
	if(err == EV3_OK) {

		// check for active brakes
		if(drv.brakes) {

			// set brake mode
			if(drv.brake_mode)
				eng_stop[10] = 0x01;
			else
				eng_stop[10] = 0x00;

			// send command
			err = send(eng_stop);
			// and exit
			return err;
		}
		// check for active steering
		else if(drv.turn) {

			bool right = false; // direction
			int sp_high = 0;	// speed on high side
			int sp_low = 0;		// speed on low side
			
			/* limit turn value */
			if(drv.turn > 100)
				drv.turn = 100;
			else if(drv.turn < -100)
				drv.turn = -100;
			
			/* apply acc steering if needed */
			if(drv.acc_steering)
				drv.turn *= 2;

			// set direction
			if(drv.turn < 0) {
				right = true;
				drv.turn *= -1;
			}

			// handle uneven integer
			if(drv.turn%2) {
				sp_high += 1;
				if( drv.turn > 0 )
					drv.turn -= 1;
				else
					drv.turn += 1;
			}

			// split turn value between both sides
			sp_high += drv.turn / 2;
			sp_low -= drv.turn / 2;
			
			/* set turn to original value */
			if(drv.acc_steering)
				drv.turn /= 2;

			// add current speed
			if(drv.speed >= 0) {
				sp_high += drv.speed;
				sp_low += drv.speed;
			} else {
				sp_high -= drv.speed;
				sp_low -= drv.speed;
			}

			// correct high values
			if(sp_high > 100) {
				sp_low -= sp_high - 100;
				sp_high = 100;
			} else if(sp_high < -100) {
				sp_low += sp_high + 100;
				sp_high = -100;
			}

			// correct low values
			if(sp_low <= -100)
				sp_low = -100;
			else if(sp_low >= 100)
				sp_low = 100;

			// correct direction
			if(drv.speed < 0) {
				sp_high *= -1;
				sp_low *= -1;
			}

			// set direction to engines
			if(right) {
				eng_right[11] = convertSpeed(sp_low);
				eng_left[11] = convertSpeed(sp_high);
			} else {
				eng_right[11] = convertSpeed(sp_high);
				eng_left[11] = convertSpeed(sp_low);
			}

			// send first command...
			err = send(eng_right);
			// ...check for error...
			if(err == EV3_OK){
				// ...wait for the EV3...
				usleep(500);
				// ...and send the second command
				err = send(eng_left);
			}
		}
		// check if we are really doing something
		else if(drv.speed) {
			// set same speed to both engines
			eng_lr[11] = convertSpeed(drv.speed);
			// send command
			err = send(eng_lr);
		}
		// else coast
		else {
			// set brake command to coast
			eng_stop[10] = 0x00;
			// send command
			err = send(eng_stop);
		}

	} // if !err

	return err;
} // drive_t drv
Beispiel #20
0
void main (void) {
	vuint32_t i = 0;
	initModesAndClock(); /* Initialize mode entries and system clock */
	disableWatchdog(); /* Disable watchdog */
	initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
	
	initADC();
	initLED();
	
	//can stuff	
	initDSPI_1();                /* Initialize DSPI_1 as Slave SPI and init CTAR0 */
	canSetup();
	
	initEnergizeButton();
	
	energizePacket.ID = ENERGIZE_ID;
	energizePacket.DATA.W[0] = 0xFFFFFFFF;
	energizePacket.DATA.W[1] = 0xFFFFFFFF;
	
	deenergizePacket.ID = DEENERGIZE_ID;
	deenergizePacket.DATA.W[0] = 0x00000000;
	deenergizePacket.DATA.W[1] = 0x00000000;
	
	torquePacket.ID = TORQUE_ID;
	speedPacket.ID = SPEED_ID;
	
	shutdownPacket.ID = SHUTDOWN_ID;
	shutdownPacket.DATA.W[0] = 0xDEADBEEF;
	shutdownPacket.DATA.W[1] = 0xDEADBEEF;
	
	/* Loop forever */
	while (1) 
	{
		prevEnergizeButton = energizeButton;
		getEnergizeButton();
		switch (currentState) {
			case NOT_ENERGIZED:
				if (!energizeButton && prevEnergizeButton) {
					currentState = ENERGIZED;
					canSend(energizePacket);
				}
				break;
			case ENERGIZED:
				if (!energizeButton && prevEnergizeButton) {
					currentState = NOT_ENERGIZED;
					canSend(deenergizePacket);
				} else {
					getADC();
					processAnalog();
					if (imp_count < 2 || torque0 < 20) {
						convertSpeed();
						convertTorque();
						if (MODE == 0) {
							canSend(speedPacket);
						} else {
							canSend(torquePacket);
						}
					} else {
						currentState = SHUTDOWN;
						canSend(deenergizePacket);
					}
				}
				break;
			case SHUTDOWN:
				canSend(shutdownPacket);
				SHUTDOWN_CIRCUIT = TRUE;
				break;
		}
		
		toLED(currentState);
		
		//canReceive();
		
		delay();
		i++;
	}
}
/* void KiwiDrive::drive(double xcomp, double ycomp, double yaw){
	
	rightWheelSpeed = ((xcomp/3.0)+(0.57735*ycomp)+(8.0*yaw/93.0));
	leftWheelSpeed = ((xcomp/3.0)-(0.57735*ycomp)+(8.0*yaw/93.0));
	frontWheelSpeed = ((-2.0*xcomp/3.0)+(8.0*yaw/93.0));
	
	leftServo.writeMicroseconds(convertSpeed(leftWheelSpeed,leftDirs));
	rightServo.writeMicroseconds(convertSpeed(rightWheelSpeed,rightDirs));
	frontServo.writeMicroseconds(convertSpeed(frontWheelSpeed,frontDirs));

} */
int KiwiDrive::getLeft(double xcomp, double ycomp, double yaw){
	leftWheelSpeed = ((xcomp/3.0)-(0.57735*ycomp)+(8.0*yaw/93.0));
	return convertSpeed(leftWheelSpeed,leftDirs);
}
int KiwiDrive::getRight(double xcomp, double ycomp, double yaw){
	rightWheelSpeed = ((xcomp/3.0)+(0.57735*ycomp)+(8.0*yaw/93.0));
	return convertSpeed(rightWheelSpeed,rightDirs);
}
int KiwiDrive::getFront(double xcomp, double ycomp, double yaw){
	frontWheelSpeed = ((-2.0*xcomp/3.0)+(8.0*yaw/93.0));
	return convertSpeed(frontWheelSpeed,frontDirs);
}