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

--- 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);