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); }
void initializeRobot() { // Sensors are config'ed and setup by RobotC (need to stabalize). Servo_SetSpeed(servo_IR, 10); // maximum speed! Servo_SetSpeed(servo_claw, 10); // maximum speed! Servo_SetSpeed(servo_ramp, 100); // slowly update so ramp doesn't release. Servo_Rotate(servo_IR, g_IRServoExtended); // will fold back up in tele-op Servo_Rotate(servo_claw, g_clawServoExtended); // will be folded in tele-op Servo_Rotate(servo_ramp, g_rampServoDefault); // stops ramp from deploying Motor_SetMaxSpeed(g_FullRegulatedPower); Motor_ResetEncoder(motor_L); Motor_ResetEncoder(motor_R); Motor_ResetEncoder(motor_lift); // Wait this long so the claw & IR servos get to update. // The ramp-release servo shouldn't move; the long update time // is to prevent sudden jerks that might release the ramp. // We don't need to wait for the IR sensor to stabalize since // the robot doesn't read from it until it's at the first column, // which should be ample time for RobotC to setup the sensor. Time_Wait(10); return; }
task Hopper() { Joystick_WaitForStart(); while (true) { //if (servo_turntable_pos<pos_servo_turntable_BL) { // servo_turntable_pos = pos_servo_turntable_BL; //} else if (servo_turntable_pos>pos_servo_turntable_BR) { // servo_turntable_pos = pos_servo_turntable_BR; //} if (lift_pos<pos_hopper_safety_down) { servo_turntable_pos = pos_servo_turntable_F; } if (hopper_pos<pos_hopper_safety_side) { servo_turntable_pos = pos_servo_turntable_F; } servo_turntable_pos = pos_servo_turntable_F; Servo_SetPosition(servo_turntable, (int)round(servo_turntable_pos)); if (lift_pos < pos_hopper_safety_down) { if (hopper_pos > (pos_servo_hopper_down+3)) { int lift_target_buf = lift_target; int hopper_target_buf = hopper_target; while (hopper_pos > (pos_servo_hopper_down+3)) { lift_target = pos_hopper_safety_down; is_lift_manual = false; hopper_pos = encoderToHopper(encoder_hopper); hopper_target = pos_servo_hopper_down; Servo_SetPosition(servo_hopper_A, hopper_target); Servo_SetPosition(servo_hopper_B, hopper_target); Time_Wait(5); } lift_target = lift_target_buf; hopper_target = hopper_target_buf; } } if (hopper_target < pos_servo_hopper_down) { hopper_target = pos_servo_hopper_down; } else if (hopper_target > pos_servo_hopper_goal) { hopper_target = pos_servo_hopper_goal; } Servo_SetPosition(servo_hopper_A, hopper_target); Servo_SetPosition(servo_hopper_B, hopper_target); Time_Wait(2); } }
int MmcDrv_CardInitialize(C_MMCDRV *self) { int i; unsigned char c; *GPIOA_DIR = 0x07; *GPIOA_OUPUT = 0x07; /* 初期化 */ for ( i = 0; i < 80; i++ ) { *GPIOA_OUPUT &= ~MMC_CLK; *GPIOA_OUPUT |= MMC_CLK; } *GPIOA_OUPUT &= ~MMC_CS; /* CMD0 */ MmcDrv_SendData(0x40); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0x95); MmcDrv_SendData(0xff); MmcDrv_SendData(0xff); MmcDrv_SendData(0xff); /* CMD1 */ for ( i = 0; ; i++ ) { MmcDrv_SendData(0x41); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0x00); MmcDrv_SendData(0xf9); MmcDrv_SendData(0xff); c = MmcDrv_SendData(0xff); MmcDrv_SendData(0xff); if ( c == 0 ) { break; } if ( i >= 200 ) { return FILE_ERR_NG; } Time_Wait(10); } return FILE_ERR_OK; }
task Gyro() { HTGYROstartCal(sensor_gyro); float vel_curr = 0.0; float vel_prev = 0.0; float dt = 0.0; int timer_gyro = 0; Time_ClearTimer(timer_gyro); while (true) { vel_prev = vel_curr; dt = (float)Time_GetTime(timer_gyro)/(float)1000.0; // msec to sec Time_ClearTimer(timer_gyro); vel_curr = (float)HTGYROreadRot(sensor_gyro); heading += ((float)vel_prev+(float)vel_curr)*(float)0.5*(float)dt; Time_Wait(1); } }
task main() { waitForStart(); initializeRobot(); // The amount of time the robot... // ...moves forward at an angle. const int forwardTimeA = 150; // ...turns to line up perpendicular to the center rack. const int turnTimeB = 40; // ...drives up to the peg before lifting the lift up. const int forwardTimeC = 155; // ...lifts the claw to put a ring on (row 2). const int liftTimeF = 79; // ...moves forward, putting the ring onto the peg. const int forwardTimeG = 65; // ...lowers its lift to get rid of the ring. const int liftTimeH = 55; // ...backs up and gets ready to go to a dispenser. const int backwardTimeI = 300; // ...turns to face parallel to the walls. const int turnTimeJ = 40; // ...moves forward to align itself with a dispenser. const int forwardTimeK = 100; // ...turns to face the dispenser. const int turnTimeL = 80; // ...moves forward to be under the dispenser. const int forwardTimeM = 50; Move_Forward (forwardTimeA, g_AccurateMotorPower); Turn_Left (turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeC, g_AccurateMotorPower); Lift_Lift (liftTimeF, g_AccurateMotorPower); Move_Forward (forwardTimeG, g_AccurateMotorPower); // Lift power is negative so that the lift goes DOWN, not UP. Lift_Lift (liftTimeH, (-1) * g_AccurateMotorPower); Move_Backward (backwardTimeI, g_AccurateMotorPower); // Turn power doesn't need to be negative (turns in-place). Turn_Left (turnTimeJ, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeK, g_AccurateMotorPower); Turn_Left (turnTimeL, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeM, g_AccurateMotorPower); while (true) { PlaySoundFile("moo.rso"); while(bSoundActive == true) { Time_Wait(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); } }
task Display() { typedef enum DisplayMode { DISP_FCS, // Default FCS screen. DISP_ENCODERS, // Raw encoder values. DISP_LIFT, // PID, status, mode DISP_HOPPER, // Maths variables. DISP_SENSORS, // Might need to split this into two screens. DISP_JOYSTICKS, // For convenience. TODO: Add buttons, D-pad, etc.? DISP_NUM }; Task_Spawn(displayDiagnostics); // Explicit here: this is only spawned when buttons are pressed. DisplayMode isMode = DISP_FCS; // We don't need to wait for start. ;) while (true) { Buttons_UpdateData(); switch (isMode) { case DISP_FCS : break; case DISP_ENCODERS : nxtDisplayTextLine(0, "Lift : %+6i", lift_pos); nxtDisplayTextLine(1, "Mtr L: %+6i", Motor_GetEncoder(encoder_L)); nxtDisplayTextLine(2, "Mtr R: %+6i", Motor_GetEncoder(encoder_R)); break; case DISP_SENSORS : nxtDisplayTextLine(0, "Angle: %3i", heading); nxtDisplayTextLine(1, "IR A : %3i", IR_A); nxtDisplayTextLine(2, "IR B : %3i", IR_B); nxtDisplayTextLine(3, "IR C : %3i", IR_C); nxtDisplayTextLine(4, "IR D : %3i", IR_D); nxtDisplayTextLine(5, "IR E : %3i", IR_E); break; case DISP_LIFT : string lift_manual_str; string lift_status_str; if (is_lift_manual) { lift_manual_str = "MANUAL"; } else { lift_manual_str = "PID"; } if (isDown) { lift_status_str = "DOWN"; } else { lift_status_str = "UP"; } nxtDisplayCenteredTextLine(0, "Lift-%s-%s", lift_manual_str, lift_status_str); nxtDisplayTextLine(1, "Pos: %+6i", lift_pos); nxtDisplayTextLine(2, "Tgt: %+6i", lift_target); nxtDisplayTextLine(3, "Pwr: %+3.3f", power_lift); nxtDisplayCenteredTextLine(4, "%+4i %+4i %+4i", term_P_lift, term_I_lift, term_D_lift); break; case DISP_HOPPER : nxtDisplayTextLine(0, "XYZ %+2.1f %+2.1f %3i", hopper_x_pos, hopper_y_pos, hopper_z_pos); nxtDisplayTextLine(1, "XYZ %+2.1f %+2.1f %3i", hopper_x_target, hopper_y_target, hopper_z_target); nxtDisplayTextLine(2, "k,i %+4i %+4i", hopper_theta, hopper_phi); nxtDisplayTextLine(3, "r,h %3.1f %3.1f", hopper_r, hopper_h); nxtDisplayTextLine(4, "encdr: %+6d", hopper_pos); nxtDisplayTextLine(5, "tgt : %+6d", hopper_target); break; case DISP_JOYSTICKS : nxtDisplayCenteredTextLine(0, "--Driver I:--"); nxtDisplayCenteredTextLine(1, "LX:%4i RX:%4i", joystick.joy1_x1, joystick.joy1_x2); nxtDisplayCenteredTextLine(2, "LY:%4i RY:%4i", joystick.joy1_y1, joystick.joy1_y2); nxtDisplayCenteredTextLine(4, "--Driver II:--"); nxtDisplayCenteredTextLine(5, "LX:%4i RX:%4i", joystick.joy2_x1, joystick.joy2_x2); nxtDisplayCenteredTextLine(6, "LY:%4i RY:%4i", joystick.joy2_y1, joystick.joy2_y2); break; default : nxtDisplayCenteredTextLine(2, "Debug info"); nxtDisplayCenteredTextLine(3, "for this screen"); nxtDisplayCenteredTextLine(4, "is not currently"); nxtDisplayCenteredTextLine(5, "available."); break; } if (Buttons_Released(NXT_BUTTON_L)==true) { Display_Clear(); isMode = (DisplayMode)((isMode+DISP_NUM-1)%DISP_NUM); if (isMode==DISP_FCS) { Task_Spawn(displayDiagnostics); } else { Task_Kill(displayDiagnostics); } } if (Buttons_Released(NXT_BUTTON_R)==true) { Display_Clear(); isMode = (DisplayMode)((isMode+DISP_NUM+1)%DISP_NUM); if (isMode==DISP_FCS) { Task_Spawn(displayDiagnostics); } else { Task_Kill(displayDiagnostics); } } Time_Wait(50); // MAGIC_NUM: Prevents the LCD from updating itself to death. } }
task PID() { const float kP_up = 0.082; const float kI_up = 0.017; const float kD_up = 0.00; const float kP_down = 0.002; const float kI_down = 0.005; const float kD_down = 0.00; const float I_term_decay_rate = 0.87; const int I_term_threshold = 500; int timer_loop = 0; Time_ClearTimer(timer_loop); int dt = Time_GetTime(timer_loop); lift_pos = Motor_GetEncoder(encoder_lift); float error = 0.0; float error_prev = 0.0; float error_sum = 0.0; float error_rate = 0.0; Joystick_WaitForStart(); Time_ClearTimer(timer_loop); while (true) { dt = Time_GetTime(timer_loop); Time_ClearTimer(timer_loop); error_prev = error; lift_pos = Motor_GetEncoder(encoder_lift); if (is_lift_manual == false) { if (lift_target < pos_lift_bottom) { lift_target = pos_lift_bottom; } if (lift_target > pos_lift_top) { lift_target = pos_lift_top; } error = lift_target - lift_pos; if (error > 0) { isDown = false; } else { isDown = true; } error_sum *= I_term_decay_rate; if (error < I_term_threshold) { error_sum += error * (float)dt; } error_rate = (error - error_prev) / (float)dt; term_P_lift = error; term_I_lift = error_sum; term_D_lift = error_rate; switch (isDown) { case true : term_P_lift *= kP_down; term_I_lift *= kI_down; term_D_lift *= kD_down; break; case false : term_P_lift *= kP_up; term_I_lift *= kI_up; term_D_lift *= kD_up; break; } power_lift = term_P_lift + term_I_lift + term_D_lift; } else { lift_target = lift_pos; power_lift = power_lift_temp; } if (abs(power_lift)<20) { power_lift = 0; } if (isLiftFrozen) { power_lift = 0; } if (isReset == false) { if (power_lift>0 && lift_pos>pos_lift_top) { power_lift = 0; } else if (power_lift<0 && lift_pos<pos_lift_bottom) { power_lift = 0; } } else { Motor_ResetEncoder(encoder_lift); } Motor_SetPower(power_lift, motor_lift_A); Motor_SetPower(power_lift, motor_lift_B); Time_Wait(2); } }
task main() { // The IR signal strengh in all 5 directions. int IRdirA = 0; int IRdirB = 0; int IRdirC = 0; int IRdirD = 0; int IRdirE = 0; waitForStart(); initializeRobot(); // The amount of time the robot... const int forwardTimeAA = 25; const int turnTimeA = 50; const int forwardTimeA = 170; const int turnTimeB = 110; const int forwardTimeB = 100; const int liftTimeB = 45; const int forwardTimeCA = 110; //TODO const int forwardTimeCB = 40; //TODO const int turnTimeD = 152; const int forwardTimeD = 110; const int liftTimeD = 135; const int forwardTimeE = 95; //TODO const int turnTimeF = 112; const int forwardTimeF = 80; const int liftTimeF = 47; const int liftTimeG = 30; //TODO const int backwardTimeG = 100; //TODO const int turnTimeG = 70; //TODO const int forwardTimeG = 20; //TODO const int liftTimeH = 50; //TODO const int backwardTimeH = 90; //TODO const int turnTimeH = 100; //TODO const int forwardTimeH = 70; //TODO const int liftTimeI = 30; //TODO const int backwardTimeI = 130; //TODO const int turnTimeI = 70; //TODO const int forwardTimeI = 170; //TODO const int forwardTimeJ = 50; //TODO const int turnTimeK = 90; //TODO const int liftTimeK = 30; //TODO const int forwardTimeK = 50; //TODO Move_Forward (forwardTimeAA, g_AccurateMotorPower); Turn_Left (turnTimeA, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeA, g_AccurateMotorPower); Time_Wait(50); HTIRS2readAllDCStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE); if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold ) { Turn_Right (turnTimeB, g_AccurateMotorPower, g_AccurateMotorPower); Lift_Up (liftTimeB, g_AccurateMotorPower); Move_Forward (forwardTimeB, g_AccurateMotorPower); Lift_Down (liftTimeG, g_AccurateMotorPower); Move_Backward (backwardTimeG, g_AccurateMotorPower); Turn_Right (turnTimeG, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeG, g_AccurateMotorPower); } else { Move_Forward (forwardTimeCA, g_AccurateMotorPower); Time_Wait(50); HTIRS2readAllACStrength(infrared, IRdirA, IRdirB, IRdirC, IRdirD, IRdirE); Move_Forward (forwardTimeCB, g_AccurateMotorPower); if ( (IRdirA+IRdirB+IRdirC+IRdirD+IRdirE) > g_IRthreshold ) { Turn_Right (turnTimeD, g_AccurateMotorPower, g_AccurateMotorPower); Lift_Up (liftTimeD, g_AccurateMotorPower); Move_Forward (forwardTimeD, g_AccurateMotorPower); Lift_Down (liftTimeH, g_AccurateMotorPower); Move_Backward (backwardTimeH, g_AccurateMotorPower); Turn_Right (turnTimeH, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeH, g_AccurateMotorPower); } else { Move_Forward (forwardTimeE, g_AccurateMotorPower); Turn_Right (turnTimeF, g_AccurateMotorPower, g_AccurateMotorPower); Lift_Up (liftTimeF, g_AccurateMotorPower); Move_Forward (forwardTimeF, g_AccurateMotorPower); Lift_Down (liftTimeI, g_AccurateMotorPower); Move_Backward (backwardTimeI, g_AccurateMotorPower); Turn_Right (turnTimeI, g_AccurateMotorPower, g_AccurateMotorPower); Move_Forward (forwardTimeI, g_AccurateMotorPower); } } Move_Forward (forwardTimeJ, g_AccurateMotorPower); Turn_Right (turnTimeK, g_AccurateMotorPower, g_AccurateMotorPower); Lift_Up (liftTimeK, g_AccurateMotorPower); Move_Forward (forwardTimeK, g_AccurateMotorPower); while (true) { PlaySoundFile("moo.rso"); while(bSoundActive == true) { Time_Wait(1); } } }