Sille Van Landschoot
/
racing_robots
Racing Robots Session
Fork of racing_robots by
Revision 6:0dc4e4225881, committed 2015-02-25
- Comitter:
- pcordemans
- Date:
- Wed Feb 25 09:10:13 2015 +0000
- Parent:
- 5:355240d7126b
- Child:
- 7:a72215b1910b
- Commit message:
- implemented show stats and cleaned up the documentation
Changed in this revision
robot_logic.cpp | Show annotated file Show diff for this revision Revisions of this file |
robot_logic.h | Show annotated file Show diff for this revision Revisions of this file |
--- 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);
--- a/robot_logic.h Tue Feb 24 09:38:40 2015 +0000 +++ b/robot_logic.h Wed Feb 25 09:10:13 2015 +0000 @@ -22,29 +22,29 @@ } LedState; -/* +/** * 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. + * @param 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); -/* +/** * 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). + * @param 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); -/* +/** * Stop the robot. */ void stop(void); -/* +/** * 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 @@ -52,7 +52,7 @@ */ void sensor_calibrate(void); -/* +/** * 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. @@ -61,37 +61,51 @@ */ int line_sensor(void); -/* +/** * Initialize the PID drive control with - * the P, I and T factors. + * the P, I and D factors. * - * @p The P factor - * @i The I factor - * @d The D factor + * @param p The P factor + * @param i The I factor + * @param d The D factor */ void pid_init(int p, int i, int 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] + * @param line_position The position of the line in a range of [-100, +100] * - * @returns The turnspeed in a range of [-100, +100] + * @return The turnspeed in a range of [-100, +100] */ int pid_turn(int line_position); -// Show drive speed and sensor data +/** + *Show speed, turn and sensor data on the LCD + */ void show_stats(); -// Show the name of the robot on the display +/** + * Shows the name of the robot on the display. + * + * @param name C character string (null-terminated) with the name of the robot (max 8 chars) + */ void show_name(char * name); -// Turn a led on or off +/** + * Turn on, off or toggle a specific LED + * @param i the LED number LED_0 .. LED_7 + * @param state the LED state LED_ON, LED_OFF, LED_TOGGLE + */ void led(LedIndex i, LedState state); -// Wait for a number of milliseconds +/** + * Wait for an approximate number of milliseconds. + * + * @param milliseconds The number of milliseconds to wait. + */ void await(int milliseconds); #endif \ No newline at end of file