Diff: robot_logic.cpp

Revision:

6:0dc4e4225881

Parent:

5:355240d7126b

Child:

7:a72215b1910b

--- a/robot_logic.cpp	Tue Feb 24 09:38:40 2015 +0000
+++ b/robot_logic.cpp	Wed Feb 25 09:10:13 2015 +0000
@@ -3,7 +3,7 @@
 // Some defines
 #define MAX_SPEED 100
 #define MAX_SENSOR 100
-#define MAX_REAL_SPEED 1.0
+#define MAX_REAL_SPEED 0.3
 #define MAX 0.3
 #define MIN 0
 
@@ -12,21 +12,15 @@
 
 // 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_turnspeed = 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;
 static int internal_led_state = 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) {
+void drive(int speed)
+{
     if (speed > MAX_SPEED || speed < -MAX_SPEED) {
         printf("[ERROR] Drive speed out of range");
         return;
@@ -43,200 +37,128 @@
     }
 }
 
-/*
- * 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) {
+void turn(int turnspeed)
+{
     if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
         error("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;
+    internal_turnspeed = turnspeed;
 
-        //     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;
+    float left = internal_speed;
+    float right = internal_speed;
+
+    left -= turnspeed;
+    right += turnspeed;
 
-        //     if (left < -MAX_SPEED) {
-        //         left = -MAX_SPEED;
-        //         right = -MAX_SPEED - turnspeed;
-        //     } else if (right < -MAX_SPEED) {
-        //         right = -MAX_SPEED;
-        //         left = -MAX_SPEED + turnspeed;
-        //     }
-        // }
+    if (right < MIN)
+        right = MIN;
+    else if (right > MAX_SPEED)
+        right = MAX_SPEED;
 
-        // 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);
-    }
+    if (left < MIN)
+        left = MIN;
+    else if (left > MAX_SPEED)
+        left = MAX_SPEED;
+    m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
+    m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
 }
 
-/*
- * Stop the robot.
- */
-void stop(void) {
+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) {
+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) {
+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) {
+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) {
+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) {
+/**
+  *Show speed, turn 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%%");
+    m3pi.printf("S%d", internal_speed);
+
+    // Display turn
+    m3pi.locate(4,0);
+    m3pi.printf("T%d", internal_turnspeed);
 
     // Display line
     m3pi.locate(0, 1);
     int line = line_sensor();
-    m3pi.printf("LINE %d", line);
+    m3pi.printf("POS %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) {
+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
+    // 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) {
+void await(int milliseconds)
+{
     wait_ms(milliseconds);
 }
 
-
-void led(LedIndex i, LedState state) {
-    if(state == LED_ON){
+void led(LedIndex i, LedState state)
+{
+    if(state == LED_ON) {
         internal_led_state |= (1 << i);
-    }
-    else if(state == LED_OFF) {
+    } else if(state == LED_OFF) {
         internal_led_state &= ~(1 << i);
-    }
-    else if(state == LED_TOGGLE){
+    } else if(state == LED_TOGGLE) {
         internal_led_state ^= (1 << i);
-    }
-    else{
+    } else {
         error("Illegal LED state");
     }
     m3pi.leds(internal_led_state);