Sille Van Landschoot
/
racing_robots
Racing Robots Session
Fork of racing_robots by
Diff: robot_logic.cpp
- Revision:
- 6:0dc4e4225881
- Parent:
- 5:355240d7126b
- Child:
- 7:a72215b1910b
diff -r 355240d7126b -r 0dc4e4225881 robot_logic.cpp --- a/robot_logic.cpp Tue Feb 24 09:38:40 2015 +0000 +++ b/robot_logic.cpp Wed Feb 25 09:10:13 2015 +0000 @@ -3,7 +3,7 @@ // Some defines #define MAX_SPEED 100 #define MAX_SENSOR 100 -#define MAX_REAL_SPEED 1.0 +#define MAX_REAL_SPEED 0.3 #define MAX 0.3 #define MIN 0 @@ -12,21 +12,15 @@ // Static scope variables for keeping internal state static int internal_speed = 0; // [-100, +100] -static int internal_turn_speed = 0; // [-100, +100] +static int internal_turnspeed = 0; // [-100, +100] static int internal_p = 0; static int internal_i = 0; static int internal_d = 0; static int internal_previous_pos_of_line = 0; static int internal_led_state = 0; -/* - * Drive the robot forward or backward. - * If the robot was turning it will stop turning and drive in a straight line. - * - * @speed The speed percentage with which to drive forward or backward. - * Can range from -100 (full throttle backward) to +100 (full throttle forward). - */ -void drive(int speed) { +void drive(int speed) +{ if (speed > MAX_SPEED || speed < -MAX_SPEED) { printf("[ERROR] Drive speed out of range"); return; @@ -43,200 +37,128 @@ } } -/* - * Turn the robot left or right while driving. - * - * @turnspeed The percentage with which to turn the robot. - * Can range from -100 (full throttle left) to +100 (full throttle right). - */ -void turn(int turnspeed) { +void turn(int turnspeed) +{ if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { error("Speed out of range"); return; } - - internal_turn_speed = turnspeed; - - if (turnspeed == 0) { // Drive straight - drive(internal_speed); - } else { - // int left = 0; - // int right = 0; - // if (internal_speed > 0) { // Right | Left forward - // left = internal_speed + turnspeed / 2; - // right = internal_speed - turnspeed / 2; + internal_turnspeed = turnspeed; - // if (left > MAX_SPEED) { - // left = MAX_SPEED; - // right = MAX_SPEED - turnspeed; - // } else if (right > MAX_SPEED) { - // right = MAX_SPEED; - // left = MAX_SPEED + turnspeed; - // } - // } - // else if (internal_speed < 0) { // Right | Left backward - // left = internal_speed - turnspeed / 2; - // right = internal_speed + turnspeed / 2; + float left = internal_speed; + float right = internal_speed; + + left -= turnspeed; + right += turnspeed; - // if (left < -MAX_SPEED) { - // left = -MAX_SPEED; - // right = -MAX_SPEED - turnspeed; - // } else if (right < -MAX_SPEED) { - // right = -MAX_SPEED; - // left = -MAX_SPEED + turnspeed; - // } - // } + if (right < MIN) + right = MIN; + else if (right > MAX_SPEED) + right = MAX_SPEED; - // Compute new speeds - int right = internal_speed+turnspeed; - int left = internal_speed-turnspeed; - - // limit checks - if (right < MIN) - right = MIN; - else if (right > MAX) - right = MAX; - - if (left < MIN) - left = MIN; - else if (left > MAX) - left = MAX; - - m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED); - m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED); - } + if (left < MIN) + left = MIN; + else if (left > MAX_SPEED) + left = MAX_SPEED; + m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED); + m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED); } -/* - * Stop the robot. - */ -void stop(void) { +void stop(void) +{ m3pi.stop(); } - -/* - * Calibrate the line follow sensors. - * Take note that the pololu should be placed over the line - * before this function is called and that it will rotate to - * both sides. - */ -void sensor_calibrate(void) { +void sensor_calibrate(void) +{ m3pi.sensor_auto_calibrate(); } -/* - * Read the value from the line sensor. The returned value indicates the - * position of the line. The value ranges from -100 to +100 where -100 is - * fully left, +100 is fully right and 0 means the line is detected in the middle. - * - * @return The position of the line with a range of -100 to +100. - */ -int line_sensor(void) { +int line_sensor(void) +{ // Get position of line [-1.0, +1.0] float pos = m3pi.line_position(); return ((int)(pos * MAX_SENSOR)); } -/* - * Initialize the PID drive control with - * the P, I and T factors. - * - * @p The P factor - * @i The I factor - * @d The D factor - */ -void pid_init(int p, int i, int d) { +void pid_init(int p, int i, int d) +{ internal_p = p; internal_i = i; internal_d = d; } -/* - * Determine PID turnspeed with which the pololu should - * turn to follow the line at the given position. - * - * @line_position The position of the line in a range of [-100, +100] - * - * @returns The turnspeed in a range of [-100, +100] - */ -int pid_turn(int line_position) { +int pid_turn(int line_position) +{ float right; float left; - + float derivative, proportional, integral = 0; float power; float speed = internal_speed; - + proportional = line_position; - + // Compute the derivative derivative = line_position - internal_previous_pos_of_line; - + // Compute the integral integral += proportional; - + // Remember the last position. internal_previous_pos_of_line = line_position; - + // Compute the power power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ; - + return power; } -// Show drive speed and sensor data -void show_stats(void) { +/** + *Show speed, turn and sensor data + */ +void show_stats(void) +{ m3pi.cls(); // Clear display // Display speed m3pi.locate(0, 0); - // x The horizontal position, from 0 to 7 - // y The vertical position, from 0 to 1 - m3pi.printf("FOR 100%%"); + m3pi.printf("S%d", internal_speed); + + // Display turn + m3pi.locate(4,0); + m3pi.printf("T%d", internal_turnspeed); // Display line m3pi.locate(0, 1); int line = line_sensor(); - m3pi.printf("LINE %d", line); + m3pi.printf("POS %d", line); } -/* - * Shows the name of the robot on the display. - * - * @name C character string (null-terminated) with the name of the robot (max 8 chars) - */ -void show_name(char * name) { +void show_name(char * name) +{ m3pi.cls(); // Clear display // Display speed m3pi.locate(0, 0); - // x The horizontal position, from 0 to 7 - // y The vertical position, from 0 to 1 + // x The horizontal position, from 0 to 7 + // y The vertical position, from 0 to 1 m3pi.printf("%s", name); } - -/* - * Wait for an approximate number of milliseconds. - * - * @milliseconds The number of milliseconds to wait. - */ -void await(int milliseconds) { +void await(int milliseconds) +{ wait_ms(milliseconds); } - -void led(LedIndex i, LedState state) { - if(state == LED_ON){ +void led(LedIndex i, LedState state) +{ + if(state == LED_ON) { internal_led_state |= (1 << i); - } - else if(state == LED_OFF) { + } else if(state == LED_OFF) { internal_led_state &= ~(1 << i); - } - else if(state == LED_TOGGLE){ + } else if(state == LED_TOGGLE) { internal_led_state ^= (1 << i); - } - else{ + } else { error("Illegal LED state"); } m3pi.leds(internal_led_state);