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();
}
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;
}
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");
}
}
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();
}
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();
}
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;
}
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;
}
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;
}
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();
}
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
}
}
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();
}
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;
}
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");
}
}
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;
}
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;
}
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
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);
}