Sille Van Landschoot
/
RacingRobotsLib
Racing Robots Session
Fork of racing_robots by
robot_logic.cpp
- Committer:
- pcordemans
- Date:
- 2015-02-24
- Revision:
- 3:b5d18690f357
- Parent:
- 2:356bb8d99326
- Child:
- 5:355240d7126b
File content as of revision 3:b5d18690f357:
#include "robot_logic.h" // Some defines #define MAX_SPEED 100 #define MAX_SENSOR 100 #define MAX_REAL_SPEED 1.0 #define MAX 0.3 #define MIN 0 // Static scope variables static m3pi m3pi; // 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 > 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_REAL_SPEED*speed/MAX_SPEED); } else if (speed < 0) { m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_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). */ void turn(int turnspeed) { if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) { printf("[ERROR] Turn 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; // 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); } } /* * Stop the robot. */ 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) { 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) { // 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) { 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 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%%"); // Display line m3pi.locate(0, 1); int line = line_sensor(); 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); } /* * Wait for an approximate number of milliseconds. * * @milliseconds The number of milliseconds to wait. */ void await(int milliseconds) { wait_ms(milliseconds); }