00001 #include "coridium.h"
00002 #include "cor_hwlib.h"
00003 #include "cor_wrflash.h"
00004
00005 #include "string.h"
00006 #include "printf.h"
00007 #include <math.h>
00008
00009 #include "battery_voltage.h"
00010 #include "bumper_switch.h"
00011 #include "infrared_rangefinder.h"
00012 #include "led_and_switch.h"
00013 #include "navigation_communication.h"
00014 #include "sonar_rangefinder.h"
00015 #include "pwm_compass.h"
00016 #include "unit_conversion.h"
00017
00018
00019
00020
00021
00022 const int SYSTEM_BATTERY_PIN = 0, MICRO_BATTERY_PIN = 1;
00023
00024 const int INFRARED_LEFT_PIN = 2, INFRARED_RIGHT_PIN = 3;
00025 const int REGULATOR_OUTPUT_PIN = 4;
00026
00027
00028
00029
00030 const int SONAR_LEFT_PIN = 0, SONAR_CENTER_PIN = 1, SONAR_RIGHT_PIN = 2;
00031
00032 const int BUMPER_LEFT_PIN = 4, BUMPER_RIGHT_PIN = 5;
00033
00034 const int RED_LED_PIN = 6, GRN_LED_PIN = 7;
00035
00036 const int RESET_PIN = 8, MODE_PIN = 9;
00037
00038 const int SEROUT_PIN = 10, RTS_PIN = 11, CTS_PIN = 12;
00039
00040 const unsigned int COMPASS_PWM_PIN = 13, COMPASS_CALIBRATE_PIN = 3;
00041
00042
00043
00044
00045 const char COLON = ':', SEMICOLON = ';';
00046 const int REGULATED_LIMIT = 15000;
00047 const int PROGRAM_LOOP_PERIOD = 47619;
00048 const unsigned int NUM_VARS = 17;
00049
00050
00051
00052 const float K_SON = 0.02, K_INF = 8.0;
00053 const float SONAR_NOM = 317.275;
00054 const float INFRARED_L_NOM = 50.0, INFRARED_R_NOM = 50.0;
00055
00056 int run_mode = 1;
00057
00058
00059
00060 void selfTest();
00061 void initializePins();
00062 void systemControllerCom();
00063
00064
00065 void sendPlatformData(int *var_array[]);
00066 void formatSend(char header, int data);
00067 void computeDeltas(int left_s, int cent_s, int right_s, int left_i, int right_i);
00068 int calculateSteering();
00069 int calculateThrottle();
00070 int getAutoAvoiding();
00071 void sendToNavController();
00072
00073 int main(void) {
00074
00075
00076
00077
00078 int system_battery = 0, micro_battery = 0;
00079
00080 int infrared_left = 0, infrared_right = 0;
00081
00082
00083
00084
00085 int sonar_left = 0, sonar_center = 0, sonar_right = 0;
00086
00087 int bumper_left = 0, bumper_right = 0;
00088
00089 int reset = 0, rc_mode = 0;
00090
00091 int red_led = 1, grn_led = 0;
00092
00093 int compass_heading = 0;
00094
00095
00096
00097 int program_loop_timer = 0;
00098 int steering = 0x55, throttle = 0x55;
00099 int auto_avoid = 0;
00100
00101
00102 int *var_data[NUM_VARS];
00103
00104 var_data[0] = &system_battery;
00105 var_data[1] = µ_battery;
00106 var_data[2] = &infrared_left;
00107 var_data[3] = &infrared_right;
00108 var_data[4] = &sonar_left;
00109 var_data[5] = &sonar_center;
00110 var_data[6] = &sonar_right;
00111 var_data[7] = &bumper_left;
00112 var_data[8] = &bumper_right;
00113 var_data[9] = &reset;
00114 var_data[10] = &rc_mode;
00115 var_data[11] = &red_led;
00116 var_data[12] = &grn_led;
00117 var_data[13] = &steering;
00118 var_data[14] = &throttle;
00119 var_data[15] = &auto_avoid;
00120 var_data[16] = &compass_heading;
00121
00122 init_coridium();
00123 WAIT(20);
00124
00125
00126 selfTest();
00127
00128
00129 initializeBatteryVoltage(SYSTEM_BATTERY_PIN, MICRO_BATTERY_PIN);
00130 initializeBumperSwitch(BUMPER_LEFT_PIN, BUMPER_RIGHT_PIN);
00131 initializeInfraredRangefinder(INFRARED_LEFT_PIN, INFRARED_RIGHT_PIN);
00132 initializeLedAndSwitch(RED_LED_PIN, GRN_LED_PIN, RESET_PIN, MODE_PIN);
00133 initializeNavigationCommunication(SEROUT_PIN, RTS_PIN, CTS_PIN);
00134 initializeSonarRangefinders(SONAR_LEFT_PIN, SONAR_CENTER_PIN, SONAR_RIGHT_PIN);
00135 initializeCompass(COMPASS_PWM_PIN, COMPASS_CALIBRATE_PIN);
00136
00137 printf("0:%s;", "plat_init");
00138
00139 while(1) {
00140
00141 systemControllerCom();
00142 if(run_mode) {
00143
00144 program_loop_timer = TIMER;
00145 while ((TIMER - program_loop_timer) < PROGRAM_LOOP_PERIOD);
00146
00147
00148 compass_heading = getCompassHeading();
00149
00150 sonar_left = getSonarLeft();
00151 while ((TIMER - program_loop_timer) < 2*PROGRAM_LOOP_PERIOD);
00152 sonar_center = getSonarCenter();
00153
00154
00155 infrared_left = getInfraredLeft();
00156 infrared_right = getInfraredRight();
00157
00158 bumper_left = getBumperLeftSwitch();
00159 bumper_right = getBumperRightSwitch();
00160
00161 reset = getResetSwitch();
00162 rc_mode = getModeSwitch();
00163
00164 red_led = getLedRedState();
00165 grn_led = getLedGreenState();
00166
00167 system_battery = getSystemBattery();
00168 micro_battery = getMicroBattery();
00169
00170 while ((TIMER - program_loop_timer) < 3*PROGRAM_LOOP_PERIOD);
00171 sonar_right = getSonarRight();
00172
00173
00174 computeDeltas(sonar_left, sonar_center, sonar_right
00175 , infrared_left, infrared_right);
00176 steering = calculateSteering();
00177 throttle = calculateThrottle();
00178
00179
00180 auto_avoid = getAutoAvoiding() || bumper_left || bumper_right;
00181
00182 sendToNavController(steering, throttle, auto_avoid, rc_mode);
00183 sendPlatformData(var_data);
00184 }
00185 }
00186 }
00187
00188
00189 void selfTest() {
00190 int regulator_output = AD(REGULATOR_OUTPUT_PIN);
00191 while(fabs(regulator_output - REGULATED_LIMIT) > 500) {
00192 printf("%s%d\r\n","regulator voltage out of range ", regulator_output);
00193 regulator_output = AD(REGULATOR_OUTPUT_PIN);
00194 SLEEP(5);
00195 }
00196 }
00197
00198
00199 void systemControllerCom(int auto_avoid) {
00200
00201
00202
00203 char header = getc();
00204 switch(header) {
00205 case 'a':
00206 formatSend(header, getSystemBattery());
00207 break;
00208 case 'b':
00209 formatSend(header, getMicroBattery());
00210 break;
00211 case 'c':
00212 formatSend(header, getInfraredLeft());
00213 break;
00214 case 'd':
00215
00216 formatSend(header, getInfraredRight());
00217 break;
00218 case 'e':
00219
00220 formatSend(header, getSonarLeft());
00221 break;
00222 case 'f':
00223
00224 formatSend(header, getSonarCenter());
00225 break;
00226 case 'g':
00227
00228 formatSend(header, getSonarRight());
00229 break;
00230 case 'h':
00231
00232 formatSend(header, getBumperLeftSwitch());
00233 break;
00234 case 'i':
00235
00236 formatSend(header, getBumperRightSwitch());
00237 break;
00238 case 'j':
00239
00240 formatSend(header, getResetSwitch());
00241 break;
00242 case 'k':
00243
00244 formatSend(header, getModeSwitch());
00245 break;
00246 case 'l':
00247
00248 if(getc() == COLON) {
00249 int red = (int)getc();
00250 if(red != 0) red = 1;
00251 setLedRedState(red);
00252 }
00253 formatSend(header, getLedRedState());
00254 break;
00255 case 'm':
00256
00257 if(getc() == COLON) {
00258 int green = (int)getc();
00259 if(green != 0) green = 1;
00260 setLedGreenState(green);
00261 }
00262 formatSend(header, getLedGreenState());
00263 break;
00264 case 'n':
00265
00266
00267 formatSend(header, calculateSteering());
00268 break;
00269 case 'o':
00270
00271
00272 formatSend(header, calculateThrottle());
00273 break;
00274 case 'p':
00275
00276 formatSend(header, auto_avoid);
00277 break;
00278 case 'q':
00279
00280 formatSend(header, getCompassHeading());
00281 break;
00282 case 'r':
00283
00284 if(getc() == COLON) {
00285 run_mode = (int)getc();
00286 if(run_mode != 0) { run_mode = 1; }
00287 setLedGreenState(run_mode);
00288 }
00289 formatSend(header, run_mode);
00290 break;
00291 case 's':
00292
00293 formatSend(header, (int)getSonarRangefinderCM(getSonarLeft()));
00294 break;
00295 case 't':
00296
00297 formatSend(header, (int)getSonarRangefinderCM(getSonarCenter()));
00298 break;
00299 case 'u':
00300
00301 formatSend(header, (int)getSonarRangefinderCM(getSonarRight()));
00302 break;
00303 case 'v':
00304
00305 formatSend(header, (int)getInfraredRangefinderCM(getInfraredLeft()));
00306 break;
00307 case 'w':
00308
00309 formatSend(header, (int)getInfraredRangefinderCM(getInfraredRight()));
00310 break;
00311 case 'x':
00312
00313 formatSend(header, (int)getBatteryVoltage(getSystemBattery()));
00314 break;
00315 case 'y':
00316
00317 formatSend(header, (int)getBatteryVoltage(getMicroBattery()));
00318 break;
00319 case 'z':
00320
00321 calibrateCompass();
00322 break;
00323 default:
00324 break;
00325 }
00326 }
00327
00328 void sendPlatformData(int *var_array[]) {
00329
00330 int i;
00331
00332 for(i = 0; i < NUM_VARS; i++) { formatSend((char)(i+97), *(var_array[i])); }
00333 }
00334
00335
00336 void formatSend(char header, int data) {
00337 printf("%c:%d;", header, data);
00338 }
00339
00340 float sonar_left_delta = 0, sonar_center_delta = 0, sonar_right_delta = 0;
00341 float infrared_left_delta = 0, infrared_right_delta = 0;
00342
00343 void computeDeltas(int left_s, int cent_s, int right_s, int left_i, int right_i) {
00344
00345
00346 sonar_left_delta = fabs(getSonarRangefinderCM(left_s) - SONAR_NOM);
00347 sonar_center_delta = fabs(getSonarRangefinderCM(cent_s) - SONAR_NOM);
00348 sonar_right_delta = fabs(getSonarRangefinderCM(right_s) - SONAR_NOM);
00349 infrared_left_delta = fabs(getInfraredRangefinderCM(left_i) - INFRARED_L_NOM);
00350 infrared_right_delta = fabs(getInfraredRangefinderCM(right_i) - INFRARED_R_NOM);
00351 }
00352
00353 int calculateSteering() {
00354 static float turn = 0;
00355
00356
00357
00358
00359 if(fabs(sonar_left_delta - sonar_right_delta)
00360 > fabs(K_INF * (infrared_left_delta - infrared_right_delta))) {
00361 turn = sonar_right_delta - sonar_left_delta;
00362 } else {
00363 turn = K_INF * (infrared_right_delta - infrared_left_delta);
00364 }
00365
00366 turn = turn * (K_SON * sonar_center_delta + 1);
00367 turn *= 0.423;
00368 if(turn > 127.0) turn = 127.0;
00369 else if(turn < -127.0) turn = -127.0;
00370 return floor(turn);
00371 }
00372
00373
00374 const float FULL_THROTTLE = 270.0;
00375 const float FULL_FORWARD = 80.0;
00376 const float FULL_REVERSE = -50.0;
00377 const float LOW_SPEED = 25.0;
00378
00379 int calculateThrottle() {
00380 static float speed = 0;
00381
00382
00383 speed = sonar_left_delta;
00384 if(speed < sonar_center_delta) speed = sonar_center_delta;
00385 if(speed < sonar_right_delta) speed = sonar_right_delta;
00386 if(speed < K_INF * infrared_left_delta)
00387 speed = K_INF * infrared_left_delta;
00388 if(speed < K_INF * infrared_right_delta)
00389 speed = K_INF * infrared_right_delta;
00390
00391
00392
00393
00394 speed = FULL_THROTTLE - speed;
00395
00396 speed *= 0.2;
00397
00398 if(speed > FULL_FORWARD) speed = FULL_FORWARD;
00399 else if(speed < FULL_REVERSE) speed = FULL_REVERSE;
00400 if(speed >= 0 && speed < LOW_SPEED) speed = LOW_SPEED;
00401 if(getBumperLeftSwitch() || getBumperRightSwitch()) speed = FULL_REVERSE;
00402 return floor(speed);
00403 }
00404
00405
00406 const float AUTO_AVOID_SONAR_LIMIT = 200.0;
00407 const float AUTO_AVOID_INFRARED_LIMIT = 16.0;
00408 const int AUTO_AVOID_PERIOD = 4000000;
00409
00410 int auto_avoid_timer = 0;
00411
00412 int getAutoAvoiding() {
00413 if(sonar_left_delta > AUTO_AVOID_SONAR_LIMIT
00414 || sonar_center_delta > AUTO_AVOID_SONAR_LIMIT
00415 || sonar_right_delta > AUTO_AVOID_SONAR_LIMIT
00416 || infrared_left_delta > AUTO_AVOID_INFRARED_LIMIT
00417 || infrared_right_delta > AUTO_AVOID_INFRARED_LIMIT) {
00418 auto_avoid_timer = TIMER;
00419 }
00420 return (TIMER - auto_avoid_timer) < AUTO_AVOID_PERIOD;
00421 }
00422