robot_logic.cpp

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