Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
Revision 2:356bb8d99326, committed 2015-02-23
- Comitter:
- dwini
- Date:
- Mon Feb 23 14:37:37 2015 +0000
- Parent:
- 1:43c91152e9ce
- Child:
- 3:b5d18690f357
- Commit message:
- Add implementation of calibrate, pid and drive control. Not working as it should !
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 Mon Feb 23 12:48:22 2015 +0000 +++ b/robot_logic.cpp Mon Feb 23 14:37:37 2015 +0000 @@ -1,49 +1,106 @@ #include "robot_logic.h" -m3pi m3pi; +// Some defines +#define MAX_SPEED 100 +#define MAX_SENSOR 100 +#define MAX_REAL_SPEED 1.0 + +// Static scope variables +static m3pi m3pi; -#define MAX_SPEED 1.0 -#define MAX_TURN_SPEED 1.0 +// 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_p = 0; +static int internal_i = 0; +static int internal_d = 0; +static int internal_previous_pos_of_line = 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) { - if (speed > 100 || speed < -100) { + if (speed > MAX_SPEED || speed < -MAX_SPEED) { printf("[ERROR] Drive speed out of range"); return; } + internal_speed = speed; + if (speed == 0) { m3pi.stop(); } else if (speed > 0) { - m3pi.forward(MAX_SPEED*speed/100.0); + m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED); } else if (speed < 0) { - m3pi.backward(MAX_SPEED*-speed/100.0); + m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED); } } /* - * Turn the robot left or right. + * Turn the robot left or right while driving. * - * @speed The speed percentage with which to turn the robot. + * @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) { - if (turnspeed > 100 || turnspeed < -100) { + if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { printf("[ERROR] Turn speed out of range"); return; } - if (turnspeed == 0) { - m3pi.stop(); - } else if (turnspeed > 0) { - m3pi.right(MAX_TURN_SPEED*turnspeed/100.0); - } else if (turnspeed < 0) { - m3pi.left(MAX_TURN_SPEED*-turnspeed/100.0); + 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; + + // 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; + + // if (left < -MAX_SPEED) { + // left = -MAX_SPEED; + // right = -MAX_SPEED - turnspeed; + // } else if (right < -MAX_SPEED) { + // right = -MAX_SPEED; + // left = -MAX_SPEED + turnspeed; + // } + // } + + // 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); } } @@ -55,6 +112,16 @@ } /* + * 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) { + 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. @@ -64,13 +131,58 @@ int line_sensor(void) { // Get position of line [-1.0, +1.0] float pos = m3pi.line_position(); - return ((int)(pos * 100)); + 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) { + 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) { + 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 showStats(void) { +void show_stats(void) { m3pi.cls(); // Clear display // Display speed @@ -85,7 +197,20 @@ m3pi.printf("LINE %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) { + 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("%s", name); +} /* @@ -93,6 +218,6 @@ * * @milliseconds The number of milliseconds to wait. */ -void doWait(int milliseconds) { +void await(int milliseconds) { wait_ms(milliseconds); } \ No newline at end of file
--- a/robot_logic.h Mon Feb 23 12:48:22 2015 +0000 +++ b/robot_logic.h Mon Feb 23 14:37:37 2015 +0000 @@ -4,13 +4,16 @@ #ifndef H_ROBOT_LOGIC #define H_ROBOT_LOGIC -//typedef enum { -// LED_1 = 0, -// LED_2 = 1, -// LED_3 = 2, -// LED_4 = 3 -// //..... -//} LedIndex; +typedef enum { + LED_1 = 0, + LED_2 = 1, + LED_3 = 2, + LED_4 = 3, + LED_5 = 4, + LED_6 = 5, + LED_7 = 6, + LED_8 = 7 +} LedIndex; typedef enum { LED_ON = 0, @@ -21,37 +24,72 @@ /* * 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 [-100, +100]. + * @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); -// speed between -100% en +100% +/* + * 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); -// Stop the robot -void stop(); +/* + * Stop the robot. + */ +void stop(void); -void sensor_calibrate(); +/* + * 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); -// Read the value from the line sensor -int line_sensor(); +/* + * 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); -// PID drive control +/* + * 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); - -// returns turnspeed +/* + * 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); // Show drive speed and sensor data -void showStats(); +void show_stats(); -// Show text on display -void display(char* fmt, ...); +// Show the name of the robot on the display +void show_name(char * name); + // Turn a led on or off -//void led(LedIndex i, LedState state); +void led(LedIndex i, LedState state); // Wait for a number of milliseconds void await(int milliseconds);