int main(){
initialize_board();
enable_motors(); // bring H-bridges of of standby
set_led(GREEN,ON);
set_led(RED,OFF);
// Forward
set_motor_all(SPEED);
printf("\nAll Motors Forward\n");
sleep(2);
// Reverse
set_motor_all(-SPEED);
printf("All Motors Reverse\n");
sleep(2);
// Stop
set_motor_all(0);
disable_motors(); //put H-bridges into standby
printf("All Motors Off\n\n");
cleanup_board();
return 0;
}
/***********************************************************************
* drive_stack
* This is the medium between the user_interface struct and the
* physical servos and motors
************************************************************************/
void* drive_stack(void* ptr){
int i; // general purpose counter for for loops
float net_drive, net_turn, net_torque_split;
// exiting condition is checked inside the switch case instead
while(1){
switch (get_state()){
case EXITING:
return NULL;
case PAUSED:
disable_motors();
// not much to do if paused!
break;
// when running, drive_stack checks if an input mode
// like mavlink, DSM2, or bluetooth is enabled
// and moves the servos and motors corresponding to
// user input and current controller mode
case RUNNING:
if(user_interface.input_mode == NONE){
cstate.servos[0]=config.serv1_center-config.turn_straight;
cstate.servos[1]=config.serv2_center+config.turn_straight;
cstate.servos[2]=config.serv3_center-config.turn_straight;
cstate.servos[3]=config.serv4_center+config.turn_straight;
for (i=1; i<=4; i++){
send_servo_pulse_normalized(i,cstate.servos[i-1]);
}
disable_motors();
break;
}
enable_motors();
// now send input to servos and motors based on drive mode and UI
// motors 2 and 3 have negative polarity in the config.txt file so that positive sign in this file =
// clockwise spin with a forward input. Eg. all of the net_drive values are positive in spin mode.
// 1 3 0 2 New version is 1 2 0 1 to match the orientation of motor wire
// 2 4 1 3 4 3 3 2 connectors on cape
switch(user_interface.drive_mode){
case LANECHANGE: // lane change maneuver
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*(0.5-config.turn_straight);
cstate.motors[0]=net_drive*config.mot1_polarity;
cstate.motors[1]=net_drive*config.mot2_polarity;
cstate.motors[2]=net_drive*config.mot3_polarity;
cstate.motors[3]=net_drive*config.mot4_polarity;
cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn;
cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn;
cstate.servos[2]=config.serv3_center-config.turn_straight+net_turn;
cstate.servos[3]=config.serv4_center+config.turn_straight+net_turn;
break;
case NORMAL_4W: // Normal 4W Steering
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*config.normal_turn_range;
net_torque_split = user_interface.turn_stick*config.torque_vec_const*net_drive;
cstate.motors[0]=(net_drive+net_torque_split)*config.mot1_polarity;
cstate.motors[1]=(net_drive+net_torque_split)*config.mot2_polarity;
cstate.motors[2]=(net_drive-net_torque_split)*config.mot3_polarity;
cstate.motors[3]=(net_drive-net_torque_split)*config.mot4_polarity;
cstate.servos[0]=config.serv1_center-config.turn_straight+net_turn;
cstate.servos[1]=config.serv2_center+config.turn_straight+net_turn;
cstate.servos[2]=config.serv3_center-config.turn_straight-net_turn;
cstate.servos[3]=config.serv4_center+config.turn_straight-net_turn;
break;
// crab, turn all wheels sideways and drive
case CRAB:
net_drive = user_interface.drive_stick*config.motor_max;
net_turn = user_interface.turn_stick*config.crab_turn_const\
*(net_drive+0.5);
cstate.motors[0]=(net_drive+net_turn)*config.mot1_polarity;
cstate.motors[1]=-(net_drive+net_turn)*config.mot2_polarity;
cstate.motors[2]=-(net_drive-net_turn)*config.mot3_polarity;
cstate.motors[3]=(net_drive-net_turn)*config.mot4_polarity;
cstate.servos[0]=config.serv1_center+config.turn_crab;
cstate.servos[1]=config.serv2_center-config.turn_crab;
cstate.servos[2]=config.serv3_center+config.turn_crab;
cstate.servos[3]=config.serv4_center-config.turn_crab;
break;
case SPIN:
net_drive = user_interface.turn_stick*config.motor_max;
cstate.motors[0]=net_drive*config.mot1_polarity;
cstate.motors[1]=-net_drive*config.mot2_polarity;
cstate.motors[2]=-net_drive*config.mot3_polarity;
cstate.motors[3]=net_drive*config.mot4_polarity;
cstate.servos[0]=config.serv1_center+config.turn_spin;
cstate.servos[1]=config.serv2_center-config.turn_spin;
cstate.servos[2]=config.serv3_center+config.turn_spin;
cstate.servos[3]=config.serv4_center-config.turn_spin;
break;
default:
printf("unknown drive_mode\n");
disable_motors();
for (i=1; i<=4; i++){
cstate.motors[i-1]=0;
cstate.servos[i-1]=0.5;
}
break;
}// end of switch(drive_mode)
// send pulses to servos and drive motors
for (i=1; i<=4; i++){
saturate_number(&cstate.servos[i-1],config.turn_min,config.turn_max);
saturate_number(&cstate.motors[i-1],-config.motor_max,config.motor_max);
set_motor(i,cstate.motors[i-1]);
send_servo_pulse_normalized(i,cstate.servos[i-1]);
}
default:
break;
} // end of switch get_state()
// run about as fast as the core itself
usleep(1000000/CONTROL_HZ);
}
return NULL;
}
/******************************************************************
* arm_controller()
* - zero out the controller
* - set the setpoint.armed_state to ARMED
* - enable motors
*******************************************************************/
int arm_controller(){
zero_out_controller();
setpoint.arm_state = ARMED;
enable_motors();
return 0;
}
