Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
Diff: robot_logic.cpp
- Revision:
- 2:356bb8d99326
- Parent:
- 0:c0ae66a0ec7a
- Child:
- 3:b5d18690f357
diff -r 43c91152e9ce -r 356bb8d99326 robot_logic.cpp --- 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