Diff: robot_logic.cpp

Revision:

8:c5dccd557aab

Parent:

7:a72215b1910b

--- a/robot_logic.cpp	Wed Feb 25 15:56:14 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,163 +0,0 @@
-#include "robot_logic.h"
-
-// Some defines
-#define MAX_SPEED 100
-#define MAX_SENSOR 100
-#define MAX_REAL_SPEED 0.75
-
-#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_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;
-
-void drive(int speed)
-{
-    if (speed > MAX_SPEED || speed < -MAX_SPEED) {
-        error("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);
-    }
-}
-
-void turn(int turnspeed)
-{
-    if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
-        error("Turn out of range");
-        return;
-    }
-    internal_turnspeed = turnspeed;
-
-    float left = internal_speed;
-    float right = internal_speed;
-
-    left -= turnspeed;
-    right += turnspeed;
-
-    if (right < MIN)
-        right = MIN;
-    else if (right > MAX_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);
-}
-
-void stop(void)
-{
-    m3pi.stop();
-}
-void sensor_calibrate(void)
-{
-    m3pi.sensor_auto_calibrate();
-}
-
-int line_sensor(void)
-{
-    // Get position of line [-1.0, +1.0]
-    float pos = m3pi.line_position();
-    return ((int)(pos * MAX_SENSOR));
-}
-
-void pid_init(int p, int i, int d)
-{
-    internal_p = p;
-    internal_i = i;
-    internal_d = d;
-}
-
-int pid_turn(int line_position)
-{
-    float derivative, proportional, integral = 0;
-    float power;
-
-    proportional = line_position / 100.0;
-
-    // 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)) ;
-
-    power = power * MAX_SPEED;
-    if (power < -MAX_SPEED)
-        power = -MAX_SPEED;
-    else if (power > MAX_SPEED)
-        power = MAX_SPEED;
-    return power ;
-}
-
-void show_stats(void)
-{
-    m3pi.cls();          // Clear display
-
-    // Display speed
-    m3pi.locate(0, 0);
-    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("POS %d", line);
-}
-
-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);
-}
-
-void await(int milliseconds)
-{
-    wait_ms(milliseconds);
-}
-
-void led(LedIndex i, LedState state)
-{
-    if(state == LED_ON) {
-        internal_led_state |= (1 << i);
-    } else if(state == LED_OFF) {
-        internal_led_state &= ~(1 << i);
-    } else if(state == LED_TOGGLE) {
-        internal_led_state ^= (1 << i);
-    } else {
-        error("Illegal LED state");
-    }
-    m3pi.leds(internal_led_state);
-}
\ No newline at end of file