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; }