task Debug()
{
while (true)
{
// Store sensor values in globals so they can be viewed in the debugger
liftPotVal = SensorValue[liftPot];
_backLeftDriveEncoder = nMotorEncoder[backLeftDrive];
_backRightDriveEncoder = nMotorEncoder[backRightDrive];
gyroValue = GyroGetAngle();
gyroValueRaw = SensorValue[gyro];
sonarValue = SensorValue[stashSonar];
leftFrontLineValue = SensorValue[leftFrontLine];
seeLine = SensorValue[leftFrontLine] < 2300;
if (seeLine)
lineNumber++;
wait1Msec(25);
}
}
void mainControlDisplayTest(void)
{
while(expanderJoyFiltered()!=JOY_LEFT)
{
ssd1306ClearScreen();
ssd1306PrintInt(10, 5, "speed dist = ",(int) (speed_control.current_distance * 100), &Font_5x8);
ssd1306PrintInt(10, 15, "follow err = ",(int) (wall_follow_control.follow_error), &Font_5x8);
ssd1306PrintInt(10, 25, "right_dist = ",(int) (position_control.end_control * 100), &Font_5x8);
ssd1306PrintInt(10, 35, "gyro = ",(int16_t) GyroGetAngle(), &Font_5x8);
ssd1306PrintInt(10, 45, "left PWM = ",(int16_t) transfert_function.left_motor_pwm, &Font_5x8);
ssd1306PrintInt(10, 55, "right PWM = ",(int16_t) transfert_function.right_motor_pwm, &Font_5x8);
// bluetoothPrintf("pwm right :%d \t %d \n",(int)transfert_function.right_motor_pwm, (int)(follow_control.follow_error*100));
// bluetoothPrintInt("error", follow_control.follow_error);
// transfert_function.right_motor_pwm = (speed_control.speed_command - (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
// transfert_function.left_motor_pwm = (speed_control.speed_command + (position_control.position_command + follow_control.follow_command)) * transfert_function.pwm_ratio;
ssd1306Refresh();
}
pid_loop.start_state = FALSE;
telemetersStop();
motorsSleepDriver(ON);
}
