void UpdateMotor(uint8 Motor_Command)
{
switch(Motor_Command)
{
/* These commands are pre-defined. The accelerometer server
sends the same commands based on the direction of acceleration.
These commands are then used to control the motor */
case FORWARD:
Control_Reg_Motor_1_Write(FRONT);
PWM_Motor_1_Start();
Control_Reg_Motor_2_Write(FRONT);
PWM_Motor_2_Start();
Clock_PWM_Start();
break;
case REVERSE:
Clock_PWM_Stop();
Control_Reg_Motor_1_Write(REAR);
PWM_Motor_1_Start();
Control_Reg_Motor_2_Write(REAR);
PWM_Motor_2_Start();
Clock_PWM_Start();
break;
case LEFT:
Clock_PWM_Stop();
Control_Reg_Motor_1_Write(REAR);
PWM_Motor_1_Start();
Control_Reg_Motor_2_Write(FRONT);
PWM_Motor_2_Start();
Clock_PWM_Start();
break;
case RIGHT:
Clock_PWM_Stop();
Control_Reg_Motor_1_Write(FRONT);
PWM_Motor_1_Start();
Control_Reg_Motor_2_Write(REAR);
PWM_Motor_2_Start();
Clock_PWM_Start();
break;
case STOP:
PWM_Motor_1_Stop();
PWM_Motor_2_Stop();
Clock_PWM_Stop();
break;
default:
PWM_Motor_1_Stop();
PWM_Motor_2_Stop();
Clock_PWM_Stop();
}
}
void init() {
// 5 second delay before we start everything up
//CyDelay(5000);
// Initialize variables
events = 0; // no pending events initially
LED0_Write(0); // LED off, turns on when we're done initializing
resetMode = FALSE;
// Enable global interrupts
CyGlobalIntEnable;
// Initialize and start hardware components:
// computer uart
UART_Computer_Start();
Comp_RX_ISR_StartEx(CompRxISR);
// drive
Clock_PWM_Start();
PWM_Drive_Start();
PWM_Drive_WriteCompare1(SERVO_NEUTRAL);
PWM_Drive_WriteCompare2(SERVO_NEUTRAL);
// video
PWM_Video_Start();
PWM_Video_WriteCompare1(VIDEO1);
PWM_Video_WriteCompare2(VIDEO1);
// clock for all other uart modules
UARTClk_Start();
// turret
UART_Turret_Start();
TurretRxIsr_StartEx(TurretRxISR);
pololuControl_turnMotorOff(POLOLUCONTROL_TURRET);
// shoulder uart
UART_Shoulder_Start();
ShoulderRxIsr_StartEx(ShoulderRxISR);
pololuControl_turnMotorOff(POLOLUCONTROL_SHOULDER);
// elbow uart
UART_Elbow_Start();
ElbowRxIsr_StartEx(ElbowRxISR);
pololuControl_turnMotorOff(POLOLUCONTROL_ELBOW);
// forearm uart
UART_Forearm_Start();
ForearmRxIsr_StartEx(ForearmRxISR);
pololuControl_turnMotorOff(POLOLUCONTROL_FOREARM);
// science uart
UART_ScienceMCU_Start();
ScienceRxIsr_StartEx(ScienceRxISR);
// camera pan/tilt servo
PWM_PanTilt_Start();
PWM_PanTilt_WriteCompare1(SERVO_NEUTRAL);
PWM_PanTilt_WriteCompare2(SERVO_NEUTRAL);
// hand (don't move)
hand_a_Write(0);
hand_b_Write(0);
// excavator
PWM_Excavator_Start();
PWM_Excavator_WriteCompare(SERVO_NEUTRAL); // excavator
// Heartbeat ISR
heartbeatIsr_StartEx(HeartbeatISR);
// sample box pwm
PWM_BoxLid_Start();
PWM_BoxLid_WriteCompare(SERVO_NEUTRAL);
// init chutes - always enable and turn off all
chute_en_Write(1);
chute1a_Write(0);
chute1b_Write(0);
chute2a_Write(0);
chute2b_Write(0);
chute3a_Write(0);
chute3b_Write(0);
chute4a_Write(0);
chute4b_Write(0);
chute5a_Write(0);
chute5b_Write(0);
chute6a_Write(0);
chute6b_Write(0);
// dynamixel relay
dynamixel_relay_Write(1);
// electromagnet initially off
electromagnet_Write(0);
// rc camera initially disabled
rc_cam_en_Write(0);
LED0_Write(1); // done initializing
}