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 }