task main()
{
Task_Kill(displayDiagnostics);
Display_Clear();
initializeGlobalVariables(); // Defined in "initialize.h", this intializes all struct members.
const int finish_delay = 4*1000; // MAGIC_NUM: milliseconds to delay when program ends. // TODO: Figure out why the delay is nowhere near this.
TFileHandle IO_handle;
TFileIOResult IO_result;
const string filename = "_reset_pods.txt";
const string filename_temp = "_reset_pods_tmp.txt"; // temp makes the filename too long??
int file_size = 0;
short rotation[POD_NUM] = {0,0,0,0};
// Read in all the values of the pods from a text file.
OpenRead(IO_handle, IO_result, filename, file_size); // TODO: Add more error handling.
if (IO_result==ioRsltSuccess) {
nxtDisplayTextLine(0, "Read from file");
nxtDisplayTextLine(1, "\"_reset_pods.txt\"");
} else if (IO_result==ioRsltFileNotFound) {
OpenRead(IO_handle, IO_result, filename_temp, file_size); // TODO: Add more error handling.
if (IO_result==ioRsltSuccess) {
nxtDisplayTextLine(0, "Read from file");
nxtDisplayTextLine(1, "\"_pods_tmp.txt\"");
} else if ( (IO_result==ioRsltFileNotFound) ||
(IO_result==ioRsltNoSpace) ||
(IO_result==ioRsltNoMoreFiles) ) {
nxtDisplayTextLine(2, "No data file.");
nxtDisplayTextLine(3, "Terminating.");
Time_Wait(finish_delay);
return; // The only way to break out of this function?
}
} else if ((IO_result==ioRsltNoSpace)||(IO_result==ioRsltNoMoreFiles)) {
nxtDisplayTextLine(2, "No data file.");
nxtDisplayTextLine(3, "Terminating.");
Time_Wait(finish_delay);
return; // The only way to break out of this function?
}
for (int i=POD_FR; i<(int)POD_NUM; i++) {
ReadShort(IO_handle, IO_result, rotation[i]);
}
Close(IO_handle, IO_result);
nxtDisplayTextLine(3, "FL:%5d FR:%5d", rotation[POD_FL], rotation[POD_FR]);
nxtDisplayTextLine(4, "BL:%5d BR:%5d", rotation[POD_BL], rotation[POD_BR]);
nxtDisplayCenteredTextLine(6, "Press [ENTER] to");
nxtDisplayCenteredTextLine(7, "end program.");
do {
Buttons_UpdateData();
Time_Wait(100); // MAGIC_NUM: Arbitrary LCD refresh delay.
} while (Buttons_Released(NXT_BUTTON_YES)==false);
}
task main() {
initializeGlobalVariables();
servoPrep();
Task_Spawn(t_liftEncoder);
Joystick_WaitForStart();
while (true) {
Joystick_UpdateData();
// driving segment
// for full power, press start controller 1
if (Joystick_Button(BUTTON_START)) {
isFullPower = true;
}
// for fine tuning, press back on controller 1
else if (Joystick_Button(BUTTON_BACK)) {
isFullPower = false;
}
// full power segment
if(isFullPower) {
drivePowerL = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_1);
drivePowerR = Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_1);
}
// fine tuning section
else {
drivePowerL = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_1) / 2;
drivePowerR = Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_1) / 2;
}
// the Left Joystick on Controller 1 controls the left wheel
motor[leftWheel] = drivePowerL;
// the Right Joystick on Controller 1 controls the right wheel
motor[rightWheel] = drivePowerR;
// for lift
// Holding RB on Controller 1 raises the lift
if(Joystick_Button(BUTTON_RB, CONTROLLER_1)) {
isLift = true;
Task_Spawn(t_raiseLift);
}
// Releasing RB on Controller 1 stops the lift
else if(Joystick_ButtonReleased(BUTTON_RB, CONTROLLER_1)) {
isLift = false;
Task_Kill(t_raiseLift);
Task_Spawn(t_stopLift);
}
// Holding LB on Controller 1 lowers the lift
else if(Joystick_Button(BUTTON_LB, CONTROLLER_1)) {
isLift = true;
Task_Spawn(t_lowerLift);
}
// Releasing LB on Controller 1 stops the lift
else if(Joystick_ButtonReleased(BUTTON_LB, CONTROLLER_1)) {
isLift = false;
Task_Kill(t_lowerLift);
Task_Spawn(t_stopLift);
}
// pressing B on Controller 1 puts lift in low goal pos
/*
else if(Joystick_Button(BUTTON_B, CONTROLLER_1) && !isLift) {
Task_Spawn(t_raiseLiftLow);
goalPos = goalPosLow;
}
pressing X on Controller 1 puts lift in middle goal pos
else if(Joystick_Button(BUTTON_X, CONTROLLER_1) && !isLift) {
Task_Spawn(t_raiseLiftMiddle);
goalPos = goalPosMid;
}
// pressing Y on Controller 1 puts lift in high goal pos
else if(Joystick_Button(BUTTON_Y, CONTROLLER_1) && !isLift) {
Task_Spawn(t_raiseLiftHigh);
goalPos = goalPosHigh;
}
// pressing A on Controller 1 lowers lift to ground
else if(Joystick_Button(BUTTON_A, CONTROLLER_1) && !isLift && goalPos == goalPosLow) {
Task_Spawn(t_lowerLiftLow);
}
else if(Joystick_Button(BUTTON_A, CONTROLLER_1) && !isLift && goalPos == goalPosMid) {
Task_Spawn(t_lowerLiftMiddle);
}
else if(Joystick_Button(BUTTON_A,CONTROLLER_1) && !isLift && goalPos == goalPosHigh) {
Task_Spawn(t_lowerLiftHigh);
}
// make a kill lift button
*/
// loop controlling the pickup
// Holding RT on Controller 1 picks up balls
if(Joystick_Button(BUTTON_RT, CONTROLLER_1)) {
Task_Kill(t_reversePickup);
Task_Spawn(t_startPickup);
}
// Releasing RT on Controller 1 stops the pickup
if(Joystick_ButtonReleased(BUTTON_RT, CONTROLLER_1)) {
Task_Kill(t_startPickup);
Task_Spawn(t_stopPickup);
}
// Holding LT on Controller 1 releases balls from the pickup
else if(Joystick_Button(BUTTON_LT, CONTROLLER_1)) {
Task_Kill(t_startPickup);
Task_Spawn(t_reversePickup);
}
// Releasing LT on Controller 1 stops the pickup
else if(Joystick_ButtonReleased(BUTTON_LT, CONTROLLER_1)) {
Task_Kill(t_reversePickup);
Task_Spawn(t_stopPickup);
}
// for clamp
// Pressing Button A on Controller 2 drops the clamp
if(Joystick_Button(BUTTON_A, CONTROLLER_2)) {
Task_Kill(t_raiseClamp);
Task_Spawn(t_dropClamp);
}
// Pressing Button B on Controller 2 raises the clamp
if(Joystick_Button(BUTTON_B, CONTROLLER_2)) {
Task_Kill(t_dropClamp);
Task_Spawn(t_raiseClamp);
}
// for the drop servo
// Pressing X on Controller 2 drops the balls from the basket
if(Joystick_Button(BUTTON_X, CONTROLLER_2)) {
Task_Kill(t_resetDrop);
Task_Spawn(t_dropBall);
}
// Pressing Y on Controller 2 resets the drop servo so balls doon't fall out of the basket
if(Joystick_Button(BUTTON_Y, CONTROLLER_2)) {
Task_Kill(t_dropBall);
Task_Spawn(t_resetDrop);
}
wait1Msec(1);
}
}
task main()
{
typedef enum MotorDirection {
DIRECTION_NONE = 0,
DIRECTION_IN,
DIRECTION_OUT
};
typedef enum PickupPos {
PICKUP_RETRACT = 0,
PICKUP_LARGE,
PICKUP_SMALL,
PICKUP_KICK
};
initializeGlobalVariables();
initializeRobotVariables();
MotorDirection pickup_I_direction = DIRECTION_NONE;
MotorDirection pickup_I_direction_prev = DIRECTION_NONE;
MotorDirection pickup_O_direction = DIRECTION_NONE;
MotorDirection pickup_O_direction_prev = DIRECTION_NONE;
MotorDirection clamp_direction = DIRECTION_NONE;
PickupPos pickup_pos = PICKUP_LARGE;
int servo_dump_pos = pos_servo_dump_closed;
float power_L = 0.0;
float power_R = 0.0;
float power_pickup_I = 0.0;
float power_pickup_O = 0.0;
float power_clamp = 0.0;
Task_Spawn(Gyro);
Task_Spawn(PID);
Task_Spawn(Display);
Task_Spawn(Hopper);
Joystick_WaitForStart();
while (true) {
Joystick_UpdateData();
HTIRS2readAllACStrength(sensor_IR, IR_A, IR_B, IR_C, IR_D, IR_E);
hopper_pos = encoderToHopper(Motor_GetEncoder(encoder_hopper));
power_L = Joystick_GenericInput(JOYSTICK_L, AXIS_Y);
power_R = Joystick_GenericInput(JOYSTICK_R, AXIS_Y);
power_lift_temp = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_2);
if (abs(power_lift_temp) > 5) {
is_lift_manual = true;
} else {
is_lift_manual = false;
}
if (Joystick_Button(BUTTON_JOYL, CONTROLLER_2) == true) {
isReset = true;
} else {
isReset = false;
}
//servo_turntable_pos -= 0.004 * Joystick_GenericInput(JOYSTICK_R, AXIS_X, CONTROLLER_2);
if (Joystick_Button(BUTTON_JOYR, CONTROLLER_2) || Joystick_ButtonReleased(BUTTON_JOYR, CONTROLLER_2)) {
servo_turntable_pos = pos_servo_turntable_F;
hopper_target = pos_servo_hopper_down;
}
if (abs(Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2))>0) {
hopper_target = hopper_pos;
hopper_target += 0.6 * Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2);
}
hopper_r = sqrt(hopper_x_target*hopper_x_target + hopper_y_target*hopper_y_target);
hopper_theta = atan2(hopper_y_target, hopper_x_target);
if (Joystick_ButtonPressed(BUTTON_B)) {
pickup_I_direction_prev = pickup_I_direction;
pickup_I_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_A)) {
pickup_I_direction_prev = pickup_I_direction;
pickup_I_direction = DIRECTION_IN;
}
if (Joystick_ButtonReleased(BUTTON_B)) {
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_ButtonReleased(BUTTON_A)) {
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_ButtonPressed(BUTTON_Y)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_O_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_X)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_O_direction = DIRECTION_IN;
}
if (Joystick_ButtonReleased(BUTTON_Y)) {
pickup_O_direction = pickup_O_direction_prev;
}
if (Joystick_ButtonReleased(BUTTON_X)) {
pickup_O_direction = pickup_O_direction_prev;
}
if (Joystick_ButtonPressed(BUTTON_RB)) {
switch (pickup_O_direction) {
case DIRECTION_NONE :
case DIRECTION_OUT :
pickup_O_direction = DIRECTION_IN;
pickup_I_direction = DIRECTION_IN;
break;
case DIRECTION_IN :
pickup_O_direction = DIRECTION_NONE;
pickup_I_direction = DIRECTION_NONE;
break;
}
}
if (Joystick_ButtonPressed(BUTTON_RT)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_I_direction_prev = pickup_I_direction;
pickup_O_direction = DIRECTION_OUT;
pickup_I_direction = DIRECTION_OUT;
}
if (Joystick_ButtonReleased(BUTTON_RT)) {
pickup_O_direction = pickup_O_direction_prev;
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_DirectionPressed(DIRECTION_R)) {
pickup_pos = PICKUP_RETRACT;
} else if (Joystick_DirectionPressed(DIRECTION_F)) {
pickup_pos = PICKUP_KICK;
} else if (Joystick_DirectionPressed(DIRECTION_L)) {
pickup_pos = PICKUP_LARGE;
} else if (Joystick_DirectionPressed(DIRECTION_B)) {
pickup_pos = PICKUP_SMALL;
}
if (Joystick_Button(BUTTON_LB)) {
clamp_direction = DIRECTION_OUT;
} else if (Joystick_Button(BUTTON_LT)) {
clamp_direction = DIRECTION_IN;
} else {
clamp_direction = DIRECTION_NONE;
}
if (Joystick_Button(BUTTON_RB, CONTROLLER_2)) {
servo_dump_pos = pos_servo_dump_open_small;
} else if (Joystick_Button(BUTTON_RT, CONTROLLER_2)) {
servo_dump_pos = pos_servo_dump_open_large;
} else {
servo_dump_pos = pos_servo_dump_closed;
}
if (pickup_I_direction==DIRECTION_IN && lift_pos<pos_dump_safety) {
servo_dump_pos = pos_servo_dump_open_feed;
}
if (Joystick_Button(BUTTON_LB, CONTROLLER_2)) {
clamp_direction = DIRECTION_IN;
}
if (Joystick_Button(BUTTON_LT, CONTROLLER_2)) {
clamp_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_X, CONTROLLER_2)) {
lift_target = pos_lift_bottom;
is_lift_manual = false;
hopper_target = pos_servo_hopper_down;
servo_turntable_pos = pos_servo_turntable_F;
isLiftFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_A, CONTROLLER_2)) {
lift_target = pos_lift_low;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_B, CONTROLLER_2)) {
lift_target = pos_lift_medium;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_Y, CONTROLLER_2)) {
lift_target = pos_lift_high;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
}
if (Joystick_ButtonPressed(BUTTON_START)) {
Servo_SetPosition(servo_hopper_A, pos_servo_hopper_goal);
Servo_SetPosition(servo_hopper_B, pos_servo_hopper_goal);
if (abs(hopper_target-pos_servo_hopper_center)<3) {
hopper_target = pos_servo_hopper_down;
} else {
hopper_target = pos_servo_hopper_center;
}
}
if (Joystick_ButtonPressed(BUTTON_BACK)) {
Servo_SetPosition(servo_hopper_A, pos_servo_hopper_center);
Servo_SetPosition(servo_hopper_B, pos_servo_hopper_center);
if (abs(hopper_target-pos_servo_hopper_goal)<3) {
hopper_target = pos_servo_hopper_down;
} else {
hopper_target = pos_servo_hopper_goal;
}
}
if (isLiftFrozen && hopper_pos < pos_servo_hopper_up) {
isLiftFrozen = false;
}
if (isHopperFrozen) {
hopper_target = pos_servo_hopper_up;
if (abs(lift_pos-lift_target)<300) {
hopper_target = pos_servo_hopper_goal;
isHopperFrozen = false;
}
}
switch (pickup_I_direction) {
case DIRECTION_NONE :
power_pickup_I = 0;
break;
case DIRECTION_IN :
power_pickup_I = 100;
break;
case DIRECTION_OUT :
power_pickup_I = -100;
break;
}
switch (pickup_O_direction) {
case DIRECTION_NONE :
power_pickup_O = 0;
break;
case DIRECTION_IN :
power_pickup_O = 100;
break;
case DIRECTION_OUT :
power_pickup_O = -100;
break;
}
switch (clamp_direction) {
case DIRECTION_NONE :
power_clamp = 0;
break;
case DIRECTION_IN :
power_clamp = 100;
break;
case DIRECTION_OUT :
power_clamp = -100;
break;
}
Motor_SetPower(power_L, motor_LT);
Motor_SetPower(power_L, motor_LB);
Motor_SetPower(power_R, motor_RT);
Motor_SetPower(power_R, motor_RB);
Motor_SetPower(power_pickup_I, motor_pickup_I);
Motor_SetPower(power_pickup_O, motor_pickup_O);
Motor_SetPower(power_clamp, motor_clamp_L);
Motor_SetPower(power_clamp, motor_clamp_R);
Servo_SetPosition(servo_dump, servo_dump_pos);
// NOTE: Hopper and turntable servos should be set in the "Hopper" task.
switch (pickup_pos) {
case PICKUP_RETRACT :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_retract);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_retract);
break;
case PICKUP_LARGE :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_large);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_large);
break;
case PICKUP_SMALL :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_small);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_small);
break;
case PICKUP_KICK :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_kick);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_kick);
break;
}
Time_Wait(5);
}
}
int main( void )
{
prvSetupHardware();
/*
Create the queue used by the OLED task. Messages for display on the OLED
are received via this queue.
*/
xOLEDQueue = xQueueCreate( mainOLED_QUEUE_SIZE, sizeof( xOLEDMessage ) );
initializeGlobalVariables();
//Depricated, but included out of love <3
//DebugWorkGodDamnit DebugWork = {&panelState, &panelDeploy, &panelRetract, &batteryLevelArray, &battTempArray0, &battTempArray1, &battOverTemp, &powerConsumption, &powerGeneration};
satelliteCommsDataStruct satelliteCommsData = {&fuelLow, &battLow, &panelState, &batteryLevelArray, &battTempArray0, &battTempArray1, &fuelLevel, &thrust, &globalCount, &isMajorCycle, &processedImageData, &systemInfo, &transportDistance};
thrusterSubDataStruct thrusterSubData = {&thrust, &fuelLevel, &globalCount, &isMajorCycle};
vehicleCommsStruct vehicleCommsData = {&vehicleCommand, &vehicleResponse, &globalCount, &isMajorCycle};
oledDisplayDataStruct oledDisplayData = {&fuelLow, &battLow, &panelState, &panelDeploy, &panelRetract, &batteryLevelArray, &battTempArray0, &battTempArray1, &fuelLevel, &transportDistance};
keyboardDataStruct keyboardData = {&panelMotorSpeedUp, &panelMotorSpeedDown};
warningAlarmDataStruct warningAlarmData = {&fuelLow, &battLow, &batteryLevelArray, &battOverTemp, &fuelLevel, &globalCount, &isMajorCycle};
scheduleDataStruct scheduleData = {&globalCount, &isMajorCycle, &battOverTemp};
transportDataStruct transportData = {&globalCount};
powerSubDataStruct powerSubData = {&panelState, &panelDeploy, &panelRetract, &batteryLevelArray, &battTempArray0, &battTempArray1, &battOverTemp};
solarPanelStruct solarPanelData = {&panelState, &panelDeploy, &panelRetract, &panelMotorSpeedUp, &panelMotorSpeedDown, &globalCount, &isMajorCycle};
pirateDataStruct pirateData = {&pirateProximity};
imageCaptureDataStruct imageCaptureData = {&processedImageData, &imageFrequency};
commandDataStruct commandData = {&remoteCommand};
// Create Handles for temp tasks
solarPanelHandle = NULL;
consoleKeyboardHandle = NULL;
imageCaptureHandle = NULL;
pirateHandle = NULL;
commandHandle = NULL;
transportHandle = NULL;
/* Start the tasks */
xTaskCreate( vOLEDTask, ( signed portCHAR * ) "OLED", mainOLED_TASK_STACK_SIZE, NULL, tskIDLE_PRIORITY, NULL );
xTaskCreate(schedule, "schedule", 40, (void*)&scheduleData, mainCHECK_TASK_PRIORITY - 1, NULL);
xTaskCreate(powerSub, "powerSub", 100,(void*)&powerSubData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(solarPanelControl, "solarPanelControl", 60, (void*)&solarPanelData, mainCHECK_TASK_PRIORITY, &solarPanelHandle);
vTaskSuspend(solarPanelHandle);
xTaskCreate(satelliteComms, "satelliteComms", mainBASIC_WEB_STACK_SIZE, (void*)&satelliteCommsData, mainCHECK_TASK_PRIORITY + 1, NULL);
xTaskCreate(vehicleComms, "vehicleComms", 50, (void*)&vehicleCommsData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(thrusterSub, "thrusterSub", 60, (void*)&thrusterSubData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(oledDisplay, "oledDisplay", 120,(void*)&oledDisplayData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(consoleKeyboard, "consoleKeyboard", 60, (void*)&keyboardData, mainCHECK_TASK_PRIORITY, &consoleKeyboardHandle);
vTaskSuspend(consoleKeyboardHandle);
xTaskCreate(warningAlarm, "warningAlarm", 100,(void*)&warningAlarmData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(transport, "transport", 100,(void*)&transportData, mainCHECK_TASK_PRIORITY, NULL);
xTaskCreate(pirates, "pirates", 100,(void*)&pirateData, mainCHECK_TASK_PRIORITY, &pirateHandle);
vTaskSuspend(pirateHandle);
xTaskCreate(imageCapture, "imageCapture", 800,(void*)&imageCaptureData, 2, &imageCaptureHandle);
vTaskSuspend(imageCaptureHandle);
xTaskCreate(command, "command", 800,(void*)&commandData, 2, &commandHandle);
vTaskSuspend(commandHandle);
#if mainINCLUDE_WEB_SERVER != 0
{
/*
Create the uIP task if running on a processor that includes a MAC and PHY.
*/
if( SysCtlPeripheralPresent( SYSCTL_PERIPH_ETH ) )
{
xTaskCreate( vuIP_Task, ( signed portCHAR * ) "uIP", mainBASIC_WEB_STACK_SIZE, NULL, mainCHECK_TASK_PRIORITY - 1, NULL );
}
}
#endif
/*
Configure the high frequency interrupt used to measure the interrupt
jitter time.
*/
//vSetupHighFrequencyTimer();
/*
Start the scheduler.
*/
vTaskStartScheduler();
/* Will only get here if there was insufficient memory to create the idle task. */
return 0;
}
