void track_puck() {
float KP = 1;
float KD = 0;
if (!TRACK_PUCK) return;
int goalie_theta_est = estimate_track_puck_theta();
if (first_move) {
prev_goalie_theta_est = goalie_theta_est;
first_move = FALSE;
}
int speed = KP * goalie_theta_est - KD * (goalie_theta_est - prev_goalie_theta_est);
prev_goalie_theta_est = goalie_theta_est;
set_motor_duty_cycle(-speed, -speed);
m_usb_tx_string("--------------------TRACK PUCK --------------------\n");
print_raw_sensor_values();
print_sensor_values();
send_float("goalie_theta_est", goalie_theta_est);
send_float("speed", speed);
send_float("tgt_duty_cycle_R", -speed);
send_float("tgt_duty_cycle_L", -speed);
}
void calculate_pid()
{
// Get orientation data from IMU and depth from depth sensor
get_imu_orientation(¤t_orientation);
current_orientation.depth = get_average_depth();
/* current_orientation.pitch and .roll are controlled towards zero
* and .depth is controlled towards target_orientation.depth
*/
// update the controller with the new pitch and roll values
PID_Update (&PID_Roll, current_orientation.roll);
PID_Update (&PID_Pitch, current_orientation.pitch);
// normalize yaw difference between -180 and 180 degrees
double delta_yaw = (target_orientation.yaw - current_orientation.yaw);
delta_yaw = fmod(delta_yaw, MAX_YAW);
if (delta_yaw > MAX_YAW/2) {
delta_yaw -= MAX_YAW;
} else if (delta_yaw < -MAX_YAW/2) {
delta_yaw += MAX_YAW;
}
PID_Update (&PID_Yaw, delta_yaw);
// calculate depth difference
double delta_depth = (target_orientation.depth - current_orientation.depth);
PID_Update (&PID_Depth, delta_depth);
// calculate the force required
double Roll_Force_Needed = FACTOR_PID_ROLL_TO_FORCE * PID_Output(&PID_Roll);
double Pitch_Force_Needed = FACTOR_PID_PITCH_TO_FORCE * PID_Output(&PID_Pitch);
double Depth_Force_Needed = FACTOR_PID_DEPTH_TO_FORCE * PID_Output(&PID_Depth);
double Yaw_Force_Needed = FACTOR_PID_YAW_TO_FORCE * PID_Output(&PID_Yaw);
double Forward_Force_Needed = FACTOR_SPEED_TO_FORCE * target_orientation.speed;
/** orientation stability
* If the COM is off center we would have some sort of factors here instead of 0.5
*/
//subgroup A:
double m_left, m_right;
//subgroup B:
double mp_front_left, mp_front_right, mp_back_left, mp_back_right; //mp = motor perpendicular
stabilizing_motors_force_to_pwm ( // this calculates the pwms for yaw motors
// These are actually switched for SubZero
0.5*Yaw_Force_Needed - Forward_Force_Needed, // m_left //we might change hard-coded 0.5 ratio later
-0.5*Yaw_Force_Needed - Forward_Force_Needed, // m_right
0, 0// m_back left, back right (not used)
//0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_front_left
//-0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_front_right
//0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_back_left
//-0.2*Yaw_Force_Needed-0.7*Forward_Force_Needed, // m_back_right
&m_left, //assumptions: polarity of motors: + pointed towards front O-ring
&m_right, //positive yaw is left turn
NULL,//&m_back_left, //positive foward is forward
NULL//&m_back_right //most importantly, assume motors work like rudders kicking backwards
);
#ifdef PRIORITIZE_PITCH_OVER_DEPTH
if (ABS(current_orientation.pitch) > 30) {
Depth_Force_Needed *= 0.8;
}
else if (ABS(current_orientation.pitch) > 35) {
Depth_Force_Needed *= 0.6;
}
else if (ABS(current_orientation.pitch) > 40) {
Depth_Force_Needed *= 0.3;
}
#endif
stabilizing_motors_force_to_pwm ( // this calculates the pwms for pitch and roll motors
//0.5*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed, // m_front_left
//-0.5*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed, // m_front_right
//-0.5*Pitch_Force_Needed + 0.4*Depth_Force_Needed, // m_rear
0.25*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_front_left
-0.25*Roll_Force_Needed + 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_front_right
0.25*Roll_Force_Needed - 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_back_left
-0.25*Roll_Force_Needed - 0.2*Pitch_Force_Needed + 0.2*Depth_Force_Needed // mp_back_right
&mp_front_left, //assumptions: polarity of motors: + pointed towards water surface
&mp_front_right, //positive roll is CCW if we are looking at the back O-ring of the submarine
&mp_back_left, //positive depth is deeper
&mp_back_right //positive pitch is nose dive
);
M_LEFT = (int)m_left;
M_RIGHT = (int)m_right;
//M_BACK_LEFT = (int)m_back_left;
//M_BACK_RIGHT = (int)m_back_right;
MP_FRONT_LEFT = (int)mp_front_left;
MP_FRONT_RIGHT = (int)mp_front_right;
MP_BACK_LEFT = (int)mp_pback_left;
MP_BACK_RIGHT = (int)mp_back_right;
/** Note that motor_force_to_pwm returns a value between -400 and 400, and the factors are such that the sum of
* each factor for every motor adds up (absolutely) to 1.0. Physics son!
*/
// write the motor settings
int i;
for ( i = 0; i < NUM_MOTORS; i++ )
{
set_motor_duty_cycle(i, motor_duty_cycle[i]);
}
}
