Nico De Witte
Racing Robots Session
Dependencies: MbedJSONValue m3pi
This is the library for the Racing Robots session. It supports the M3PI robot of Polulu.
It is based on the "Arduino" principle of the init and loop function.
Just add a main.cpp file which contains:
Racing Robots main file
#include "robot_logic.h"
void init()
{
//put your initialization logic here
}
void loop()
{
//put your robot control logic here
}
Features include:
- Controlling the LEDS
- Move forward and backward
- Turn
- Read the sensor values
- Use a PID controller
Diff: robot_logic.cpp
- Revision:
- 2:356bb8d99326
- Parent:
- 0:c0ae66a0ec7a
- Child:
- 3:b5d18690f357
--- 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
