robot_logic.cpp

00001 #include "robot_logic.h"
00002 
00003 // Some defines
00004 #define MAX_SPEED 100
00005 #define MAX_SENSOR 100
00006 #define MAX_REAL_SPEED 0.3
00007 
00008 #define MIN -MAX_SPEED
00009 
00010 // Static scope variables
00011 static m3pi m3pi;
00012 
00013 // Static scope variables for keeping internal state
00014 static int internal_speed = 0;          // [-100, +100]
00015 static int internal_turnspeed = 0;     // [-100, +100]
00016 static int internal_p = 0;
00017 static int internal_i = 0;
00018 static int internal_d = 0;
00019 static int internal_previous_pos_of_line = 0;
00020 static int internal_led_state = 0;
00021 
00022 void drive(int speed)
00023 {
00024     if (speed > MAX_SPEED || speed < -MAX_SPEED) {
00025         error("Speed out of range");
00026         return;
00027     }
00028 
00029     internal_speed = speed;
00030 
00031     if (speed == 0) {
00032         m3pi.stop();
00033     } else if (speed > 0) {
00034         m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED);
00035     } else if (speed < 0) {
00036         m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED);
00037     }
00038 }
00039 
00040 void turn(int turnspeed)
00041 {
00042     if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
00043         error("Turn out of range");
00044         return;
00045     }
00046     internal_turnspeed = turnspeed;
00047 
00048     float left = internal_speed;
00049     float right = internal_speed;
00050 
00051     left -= turnspeed;
00052     right += turnspeed;
00053 
00054     if (right < MIN)
00055         right = MIN;
00056     else if (right > MAX_SPEED)
00057         right = MAX_SPEED;
00058 
00059     if (left < MIN)
00060         left = MIN;
00061     else if (left > MAX_SPEED)
00062         left = MAX_SPEED;
00063         
00064     m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
00065     m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
00066 }
00067 
00068 void stop(void)
00069 {
00070     m3pi.stop();
00071 }
00072 void sensor_calibrate(void)
00073 {
00074     m3pi.sensor_auto_calibrate();
00075 }
00076 
00077 int line_sensor(void)
00078 {
00079     // Get position of line [-1.0, +1.0]
00080     float pos = m3pi.line_position();
00081     return ((int)(pos * MAX_SENSOR));
00082 }
00083 
00084 void pid_init(int p, int i, int d)
00085 {
00086     internal_p = p;
00087     internal_i = i;
00088     internal_d = d;
00089 }
00090 
00091 int pid_turn(int line_position)
00092 {
00093     float derivative, proportional, integral = 0;
00094     float power;
00095 
00096     proportional = line_position / 100.0;
00097 
00098     // Compute the derivative
00099     derivative = line_position - internal_previous_pos_of_line;
00100 
00101     // Compute the integral
00102     integral += proportional;
00103 
00104     // Remember the last position.
00105     internal_previous_pos_of_line = line_position;
00106 
00107     // Compute the power
00108     power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;
00109 
00110     power = power * MAX_SPEED;
00111     if (power < -MAX_SPEED)
00112         power = -MAX_SPEED;
00113     else if (power > MAX_SPEED)
00114         power = MAX_SPEED;
00115     return power ;
00116 }
00117 
00118 void show_stats(void)
00119 {
00120     m3pi.cls();          // Clear display
00121 
00122     // Display speed
00123     m3pi.locate(0, 0);
00124     m3pi.printf("S%d", internal_speed);
00125 
00126     // Display turn
00127     m3pi.locate(4,0);
00128     m3pi.printf("T%d", internal_turnspeed);
00129 
00130     // Display line
00131     m3pi.locate(0, 1);
00132     int line = line_sensor();
00133     m3pi.printf("POS %d", line);
00134 }
00135 
00136 void show_name(char * name)
00137 {
00138     m3pi.cls();          // Clear display
00139 
00140     // Display speed
00141     m3pi.locate(0, 0);
00142     // x   The horizontal position, from 0 to 7
00143     // y   The vertical position, from 0 to 1
00144     m3pi.printf("%s", name);
00145 }
00146 
00147 void await(int milliseconds)
00148 {
00149     wait_ms(milliseconds);
00150 }
00151 
00152 void led(LedIndex i, LedState state)
00153 {
00154     if(state == LED_ON) {
00155         internal_led_state |= (1 << i);
00156     } else if(state == LED_OFF) {
00157         internal_led_state &= ~(1 << i);
00158     } else if(state == LED_TOGGLE) {
00159         internal_led_state ^= (1 << i);
00160     } else {
00161         error("Illegal LED state");
00162     }
00163     m3pi.leds(internal_led_state);
00164 }