Exemplo n.º 1
0
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();
		
	}
}
Exemplo n.º 2
0
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
}