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