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;
}
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_IRServoLowered); // it gets in the way
Servo_Rotate(servo_claw, g_clawServoFolded); // servo bracket gets bent
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);
// We might not even use this--the operator should have control.
// Uncomment the next lines if the robot burns and crashes.
//// 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.
//Time_Wait(10);
return;
}
void initializeRobot()
{
// Sensors are config'ed and setup by RobotC (need to stabalize).
Servo_SetSpeed(servo_IR, 20); // relatively high speed.
Servo_SetSpeed(servo_claw, 20); // relatively high speed.
Servo_SetSpeed(servo_ramp, 0); // ??? Might not work anymore.
Servo_Rotate(servo_IR, g_IRServoLowered); // it gets in the way
Servo_Rotate(servo_claw, g_clawServoFolded); // servo bracket gets bent
Servo_Rotate(servo_ramp, g_rampServoDefault); // stops ramp from deploying
Motor_SetMaxSpeed(g_FullPower);
Motor_ResetEncoder(motor_L);
Motor_ResetEncoder(motor_R);
Motor_ResetEncoder(motor_lift);
bFloatDuringInactiveMotorPWM = false;
return;
}
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);
}
}
