// turns off arm motors and resets servos to neutral position.
void resetAll() {
PWM_Video_WriteCompare1(VIDEO1);
PWM_Video_WriteCompare2(VIDEO2);
PWM_Drive_WriteCompare1(SERVO_NEUTRAL);
PWM_Drive_WriteCompare2(SERVO_NEUTRAL);
pololuControl_turnMotorOff(POLOLUCONTROL_TURRET);
pololuControl_turnMotorOff(POLOLUCONTROL_SHOULDER);
pololuControl_turnMotorOff(POLOLUCONTROL_FOREARM);
pololuControl_turnMotorOff(POLOLUCONTROL_ELBOW);
PWM_Excavator_WriteCompare(SERVO_NEUTRAL);
// init chutes - turn off all
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);
LED0_Write(1);
}
void nightRider(uint8 i)
{
turnLedsOff();
switch(i) {
case 0:
LED4_Write(0u);
break;
case 1:
case 13:
LED4_Write(0u);
LED2_Write(0u);
break;
case 2:
case 12:
LED2_Write(0u);
LED0_Write(0u);
break;
case 3:
case 11:
LED0_Write(0u);
LED1_Write(0u);
break;
case 4:
case 10:
LED1_Write(0u);
LED3_Write(0u);
break;
case 5:
case 9:
LED3_Write(0u);
LED5_Write(0u);
break;
case 6:
case 8:
LED5_Write(0u);
LED6_Write(0u);
break;
case 7:
LED6_Write(0u);
break;
default:
break;
}
}
void turnLedsOn()
{
LED0_Write(0u);
LED1_Write(0u);
LED2_Write(0u);
LED3_Write(0u);
LED4_Write(0u);
LED5_Write(0u);
LED6_Write(0u);
GPIO_0_Write(0u);
GPIO_1_Write(0u);
}
void handleAdvertisingPacketChange()
{
typedef enum advStates {
ADVSTATEPACKET0,
ADVSTATEINSERTPACKET,
} advStates;
typedef enum insertPacketStates {
ADVSTATEEPACKET1,
ADVSTATEEPACKET2
} insertPacketStates;
static advStates advState = ADVSTATEPACKET0;
static insertPacketStates insertState = ADVPACKET1;
static uint32 advTimer=0;
switch(advState)
{
case ADVSTATEPACKET0:
setupType0Adv();
if(systime > advTimer + 500)
advState = ADVSTATEINSERTPACKET;
break;
case ADVSTATEINSERTPACKET:
LED0_Write(~LED0_Read());
if(insertState == ADVSTATEEPACKET1)
{
setupType1Adv();
insertState = ADVSTATEEPACKET2;
}
else
{
setupType2Adv();
insertState = ADVSTATEEPACKET1;
}
advState = ADVSTATEPACKET0;
advTimer = systime;
break;
default:
setupType0Adv();
break;
}
}
int main()
{
CyGlobalIntEnable;
CapSense_Start();
CapSense_InitializeEnabledBaselines();
CapSense_ScanEnabledWidgets();
for(;;)
{
if(!CapSense_IsBusy())
{
LED0_Write(!CapSense_CheckIsWidgetActive(CapSense_BUTTON0__BTN));
LED1_Write(!CapSense_CheckIsWidgetActive(CapSense_BUTTON1__BTN));
CapSense_UpdateEnabledBaselines();
CapSense_ScanEnabledWidgets();
}
}
}
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
}
