int main()
{
Network network;
Accelerometer imu;
imu.bypassDrift();
motors.setToZero();
signal(SIGABRT, sigHandler);
signal(SIGINT, sigHandler);
signal(SIGKILL, sigHandler);
signal(SIGQUIT, sigHandler);
signal(SIGTERM, sigHandler);
float ypr[3];
while(true) {
if(imu.getFIFOCount() > 42) {
imu.getYawPitchRoll(ypr);
float p_computed = p_pid.compute(ypr[1], p_target), r_computed = r_pid.compute(ypr[2], r_target);
motors.setSpeed(MOTOR_FL, throttle + r_computed - p_computed);
motors.setSpeed(MOTOR_BL, throttle + r_computed + p_computed);
motors.setSpeed(MOTOR_FR, throttle - r_computed - p_computed);
motors.setSpeed(MOTOR_BR, throttle - r_computed + p_computed);
network.send(SET_MEASURED_VALUES, ypr, sizeof(float)*3, false);
}
}
exit(EXIT_SUCCESS);
}
/*Each time this is called, this pets the watchdog and computes how much the heater must be powered in order to maintain
the desired temperature. If you every want to change the internal workings of the system, this is the function where you put that
code*/
void internalStateLoop(const void *context) {
//Pet the watchdog
W.Pet();
controller.setProcessValue(internal_temp); //We won't actually read from the TMP 102.h, we'll use the most recent internal temp variable (global).
// Set the new output.
heater = controller.compute();
printf("What should the output be? %f\n", controller.compute());
// Now check for termination conditions
// 1. If the GPS lat,lon exceed the permitted bounds, cut down.
// 2. If you receive an iridum command telling you to end the flight, cut down.
// 3. If you've not received an Iridium command in a while (5 hrs), cut down.
}
void update_speed_and_heading()
{
if(distance_to_current_nav(degToRad((double)NMEA::getLatitude()), degToRad((double)NMEA::getLongitude())) < WAYPOINT_RADIUS)
go_next_nav();
nav_list_t * current_nav = get_current_nav();
bearing = compass.getHeadingXYDeg();
heading = startHeading(degToRad(NMEA::getLatitude()), degToRad(NMEA::getLongitude()), current_nav->latitude, current_nav->longitude)*(180.0/M_PI);
headingPid.setProcessValue(heading_delta(heading,bearing));
speedOverGroundPid.setProcessValue(NMEA::getSpeed());
#ifdef SPEED_PID_CALIBRATION
bearingCompensation = 0;
#else
bearingCompensation = headingPid.compute();
#endif
#ifdef BEARING_PID_CALIBRATION
speedOverGroundCompensation = 0;
#else
speedOverGroundCompensation = speedOverGroundPid.compute();
#endif
leftThrottle = ((speedOverGroundCompensation - bearingCompensation) < THROTTLE_LIMIT) ? (speedOverGroundCompensation - bearingCompensation) : THROTTLE_LIMIT;
rightThrottle = ((speedOverGroundCompensation + bearingCompensation) < THROTTLE_LIMIT) ? (speedOverGroundCompensation + bearingCompensation) : THROTTLE_LIMIT;
}
void RosAriaNode::Mas1ToSla_cb( const geometry_msgs::PointStampedConstPtr &msg)
{
// Master 1 Position
Vm1 = msg->point.x;
Xm1 = Xm1 + Vm1;
// Master force
Fk1 = msg->point.y;
// Master 1 Positive Energy
mst1_slv_cmd_P = msg->point.z;
Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
Xsprv = Xs;
Position = robot->getPose();
Xs = Position.getX();
delta = Xs - Xsprv;
Vs = PosController.compute(Xsd,Xs);
// Fs - Sum
Fs = K_force*(Xsd - Xs);
Fs1 = alpha*Fs;
Fs2 = (1-alpha)*Fs;
/*
* Master 1 - Slave Channel
*/
// Calculate Negative Energy and dissipate Active energy
if (Vm1*Fs1>0)
{
mst1_slv_cmd_N -=Vm1*Fs1;
}
else
{
//Do nothing
}
// PC:
if (mst1_slv_cmd_N+mst1_slv_cmd_P<0)
{
mst1_slv_cmd_N +=Vm1*Fs1; // backward 1 step
Xm1 = Xm1 - Vm1; // backward 1 step
// Modify Vm1
if (Fs1*Fs1>0)
Vm1 = (mst1_slv_cmd_N+mst1_slv_cmd_P)/Fs1;
else
Vm1 = 0;
//Update
Xm1 = Xm1 + Vm1;
Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
Vs = PosController.compute(Xsd,Xs);
// Modify Fs ????
mst1_slv_cmd_N -=Vm1*Fs1;
}
/*
* Slave - Master 1 Channel
*/
// Calculate Positive Energy
if (Fk1*Vs>0)
{
//sla_mst1_cmd_P += Fk1*Vs;
sla_mst1_cmd_P += Fk1*delta;
}
else
{
//Do nothing
}
/*
* Master 2 - Slave Channel
*/
// Calculate Negative Energy and dissipate Active energy
if (Vm2*Fs2>0)
{
mst2_slv_cmd_N -=Vm2*Fs2;
}
else
{
//Do nothing
}
// PC:
if (mst2_slv_cmd_N+mst2_slv_cmd_P<0)
{
mst2_slv_cmd_N +=Vm2*Fs2; // backward 1 step
Xm2 = Xm2 - Vm2; // backward 1 step
// Modify Vm1
if (Fs2*Fs2>0)
Vm2 = (mst2_slv_cmd_N+mst2_slv_cmd_P)/Fs2;
else
Vm2 = 0;
//Update
Xm2 = Xm2 + Vm2;
Xsd = Scale *(alpha*Xm1 + (1-alpha)*Xm2);// design position
Vs = PosController.compute(Xsd,Xs);
// Modify Fs ????
mst2_slv_cmd_N -=Vm2*Fs2;
}
/*
* Slave - Master 2 Channel
*/
// Calculate Positive Energy
if (Fk2*Vs>0)
{
sla_mst2_cmd_P += Fk2*Vs;
}
else
{
//Do nothing
}
//Saturation
if (Vs>MaxVel) Vs = MaxVel;
if (Vs< - MaxVel) Vs = -MaxVel;
//ROS_INFO("Velocity: %5f",Vs);
//ROS_INFO("Ref - Real - Vel : %5f -- %5f --%5f",Xsd,Xs,Vs);
ROS_INFO("Position: %5f - %5f",Xs,Xsd);
robot->setVel(Vs);
// Publisher
SlaToMas1.point.x = Fs1;
SlaToMas1.point.y = delta;
SlaToMas1.point.z = sla_mst1_cmd_P;
SlaToMas1_Pub.publish(SlaToMas1);
SlaToMas2.point.x = Fs2;
SlaToMas2.point.y = Vs;
SlaToMas2.point.z = sla_mst2_cmd_P;
SlaToMas2_Pub.publish(SlaToMas2);
}
void MotorControl::update_omega(void) {
pid->setProcessValue(_compass->measure_angle(target_angle_));
omega = pid->compute();
move(power_, move_angle_);
}
