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