00001
00002 #include "coridium.h"
00003 #include "cor_hwlib.h"
00004 #include "cor_wrflash.h"
00005
00006 #include "mem.h"
00007 #include "string.h"
00008 #include "printf.h"
00009 #include <math.h>
00010
00011 #include "accelerometer.h"
00012 #include "encoder_counter.h"
00013 #include "platform_communication.h"
00014 #include "steering_throttle.h"
00015 #include "dead_man.h"
00016
00017
00018
00019
00020
00021 const unsigned short ACC_X_AXIS_PIN = 0, ACC_Y_AXIS_PIN = 1, ACC_Z_AXIS_PIN = 2;
00022 const unsigned short GND_REF_PIN = 4;
00023
00024
00025
00026
00027 const unsigned short SPI_SCLK_PIN = 2, SPI_SS_PIN = 3
00028 , SPI_MOSI_PIN = 4, SPI_MISO_PIN = 5;
00029
00030 const unsigned short STEERING_PULSOUT_PIN = 7, THROTTLE_PULSOUT_PIN = 6;
00031 const unsigned short STEERING_PULSIN_PIN = 9, THROTTLE_PULSIN_PIN = 8;
00032
00033 const unsigned short PLATFORM_SERIN_PIN = 10;
00034 const unsigned short PLATFORM_DATA_READY_PIN = 11;
00035 const unsigned short PLATFORM_ACK_PIN = 12;
00036
00037 const unsigned short DEAD_MAN_PIN = 13;
00038
00039
00040
00041
00042 const char COLON = ':', SEMICOLON = ';';
00043 const unsigned int GROUND_REFERENCE_LIMIT = 0;
00044 const unsigned int NUM_VARS = 6;
00045 const int PROGRAM_LOOP_PERIOD = 20000;
00046 const int BRAKE_DURATION = 4000000, REVERSE_DURATION = 4000000;
00047 const int FULL_BRAKE = -100, REVERSING_CONSTANT = -50;
00048
00049
00050 int selfTest();
00051 void sendNavigationData(int *var_array[]);
00052 void formatSend(char header, int data);
00053 void systemControllerCom(int speed, int* system_steering_setpoint
00054 , int* system_throttle_setpoint, short* run_mode, short* sytem_dead_man);
00055
00056 int main(void) {
00057
00058
00059
00060
00061 int acc_x_volts = 0, acc_y_volts = 0, acc_z_volts = 0;
00062 int ground_reference = 0;
00063
00064
00065
00066
00067 int prev_encoder_counts = 0;
00068 int encoder_counts = 0;
00069 int delta_encoder_counts = 0;
00070 int normalized_counts = 0;
00071
00072 int steering_pulse_duration = 1406, throttle_pulse_duration = 1501;
00073 int steering_setpoint = 0, throttle_setpoint = 0;
00074 short braking_flag = 0;
00075 short reverse_flag = 0;
00076
00077
00078
00079
00080 int program_loop_timer = 0;
00081 int brake_duration_timer = 0;
00082 int reverse_duration_timer = 0;
00083 int state_alternator = 0;
00084
00085 int system_steering_setpoint = 0;
00086 int system_throttle_setpoint = 0;
00087 short run_mode = 0, sytem_dead_man = 1, dead_man = 1;
00088 short radio_control_mode = 0;
00089
00090
00091 int *var_data[NUM_VARS];
00092
00093 var_data[0] = &acc_x_volts;
00094 var_data[1] = &acc_y_volts;
00095 var_data[2] = &acc_z_volts;
00096 var_data[3] = &encoder_counts;
00097 var_data[4] = &steering_pulse_duration;
00098 var_data[5] = &throttle_pulse_duration;
00099
00100 init_coridium();
00101 WAIT(20);
00102
00103
00104 ground_reference = selfTest();
00105
00106 initializeEncoderCounter(SPI_SS_PIN, SPI_SCLK_PIN, SPI_MOSI_PIN, SPI_MISO_PIN);
00107
00108
00109
00110
00111 initializeSteeringAndThrottleOut(STEERING_PULSOUT_PIN, THROTTLE_PULSOUT_PIN);
00112 initializeSteeringAndThrottleIn(STEERING_PULSIN_PIN, THROTTLE_PULSIN_PIN);
00113 initializePlatformCommunication(PLATFORM_SERIN_PIN, PLATFORM_DATA_READY_PIN, PLATFORM_ACK_PIN);
00114 initializeDeadMan(DEAD_MAN_PIN);
00115
00116 printf("0:%s;", "nav_init");
00117
00118
00119 program_loop_timer = TIMER;
00120 brake_duration_timer = TIMER;
00121
00122 while(1) {
00123 radio_control_mode = getRadioControlMode();
00124 dead_man = getDeadMan();
00125
00126
00127
00128
00129
00130
00131
00132 prev_encoder_counts = encoder_counts;
00133 encoder_counts = getEncoderCounts();
00134 delta_encoder_counts = encoder_counts - prev_encoder_counts;
00135
00136 normalized_counts = ceilf((float)delta_encoder_counts * 6.375);
00137
00138
00139 systemControllerCom(delta_encoder_counts, &system_steering_setpoint
00140 , &system_throttle_setpoint, &run_mode, &sytem_dead_man);
00141
00142
00143 acc_x_volts = getAccelerometerX();
00144 acc_y_volts = getAccelerometerY();
00145 acc_z_volts = getAccelerometerZ();
00146
00147
00148 if(state_alternator == 0 || state_alternator == 4) checkForPlatformData();
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 if(sytem_dead_man && dead_man && !radio_control_mode) {
00163
00164 throttle_setpoint = 0;
00165 resetIntegralAndDerivative();
00166 } else if(radio_control_mode) {
00167
00168 if(state_alternator == 2 || state_alternator == 6)
00169 checkForSteeringAndThrottle();
00170 steering_setpoint = getSteeringPulsinDuration();
00171 throttle_setpoint = getThrottlePulsinDuration();
00172 } else if(braking_flag || reverse_flag || run_mode == 1
00173 || (run_mode == 2 && getAutoAvoidObstacle())) {
00174
00175
00176 steering_setpoint = getSteeringRecommendation();
00177 throttle_setpoint = getThrottleRecommendation();
00178
00179 if(throttle_setpoint < 0) {
00180 if(!braking_flag) {
00181 brake_duration_timer = TIMER;
00182 braking_flag = 1;
00183 } else {
00184 reverse_duration_timer = TIMER;
00185 reverse_flag = 1;
00186 }
00187 }
00188 if(braking_flag && TIMER - brake_duration_timer < BRAKE_DURATION) {
00189
00190 if((TIMER - brake_duration_timer) < BRAKE_DURATION / 2) {
00191 throttle_setpoint = FULL_BRAKE;
00192 } else {
00193
00194 resetIntegralAndDerivative();
00195 throttle_setpoint = 0;
00196 }
00197 } else if(reverse_flag && TIMER - reverse_duration_timer < REVERSE_DURATION) {
00198
00199 if((TIMER - reverse_duration_timer) < REVERSE_DURATION / 2) {
00200 if(steering_setpoint > 0) steering_setpoint = -PULSE_SPAN;
00201 else steering_setpoint = PULSE_SPAN;
00202 throttle_setpoint = REVERSING_CONSTANT;
00203 } else {
00204
00205 resetIntegralAndDerivative();
00206 throttle_setpoint = 0;
00207 braking_flag = 0;
00208 }
00209 } else {
00210 braking_flag = 0;
00211 reverse_flag = 0;
00212 }
00213 } else {
00214
00215
00216 steering_setpoint = system_steering_setpoint;
00217 if(run_mode == 0 || getThrottleRecommendation() > system_throttle_setpoint)
00218 throttle_setpoint = system_throttle_setpoint;
00219 else throttle_setpoint = getThrottleRecommendation();
00220 }
00221
00222
00223
00224 while ((TIMER - program_loop_timer) < PROGRAM_LOOP_PERIOD);
00225 program_loop_timer = TIMER;
00226
00227 steering_pulse_duration = pulsoutSteering(steering_setpoint);
00228
00229 if(!radio_control_mode)
00230 throttle_pulse_duration = pulsoutThrottle(throttle_setpoint, normalized_counts);
00231 else throttle_pulse_duration = pulsoutThrottleNoPID(throttle_setpoint);
00232
00233 if(state_alternator == 8) sendNavigationData(var_data);
00234
00235
00236
00237
00238 if(++state_alternator > 8) { state_alternator = 0; }
00239 }
00240 }
00241
00242
00243 int selfTest() {
00244
00245 int ground_reference = AD(GND_REF_PIN);
00246 while(fabs(ground_reference - GROUND_REFERENCE_LIMIT) > 100) {
00247 printf("%s%d\r\n","ground reference voltage out of range ", ground_reference);
00248 ground_reference = AD(GND_REF_PIN);
00249 SLEEP(5);
00250 }
00251 return ground_reference;
00252 }
00253
00254
00255 void sendNavigationData(int *var_array[]) {
00256
00257 int i;
00258
00259 for(i = 0 ; i < NUM_VARS; i++)
00260 formatSend((char)(i+97), *(var_array[i]));
00261 }
00262
00263
00264 void formatSend(char header, int data) {
00265 printf("%c:%d;", header, data);
00266 }
00267
00268
00269 void systemControllerCom(int speed, int* system_steering_setpoint
00270 , int* system_throttle_setpoint, short* run_mode, short* sytem_dead_man) {
00271 static short echo = 0;
00272
00273 char header = getc();
00274 switch(header) {
00275 case 'a':
00276
00277 formatSend(header, getAccelerometerX());
00278 break;
00279 case 'b':
00280
00281 formatSend(header, getAccelerometerY());
00282 break;
00283 case 'c':
00284
00285 formatSend(header, getAccelerometerZ());
00286 break;
00287 case 'd':
00288
00289 if(getc() == COLON) {
00290 char in_string[10];
00291 gets(in_string);
00292 int val = atoi(in_string);
00293 setCountRegister(val);
00294 }
00295
00296 if(echo) formatSend(header, getEncoderCounts());
00297 break;
00298 case 'g':
00299
00300 formatSend(header, speed);
00301 break;
00302 case 'h':
00303
00304 if(getc() == COLON) { setModeRegister0(getc()); }
00305 if(echo) formatSend(header, getModeRegister0());
00306 break;
00307 case 'i':
00308
00309 if(getc() == COLON) { setModeRegister1(getc()); }
00310 if(echo) formatSend(header, getModeRegister1());
00311 break;
00312 case 'j':
00313
00314 formatSend(header, getStatusRegister());
00315 break;
00316 case 'k':
00317
00318 clearEncoderCounterCounts();
00319 if(echo) formatSend(header, getEncoderCounts());
00320 break;
00321 case 'l':
00322
00323 if(getc() == COLON) {
00324 int steering_setpoint = (short)getc();
00325 if(steering_setpoint > 127)
00326 steering_setpoint -= 256;
00327 *system_steering_setpoint = ceilf(3.19 * (float)steering_setpoint);
00328 }
00329 if(echo) formatSend(header, *system_steering_setpoint);
00330 break;
00331 case 'm':
00332
00333 if(getc() == COLON) {
00334 int throttle_setpoint = (short)getc();
00335 if(throttle_setpoint > 127) throttle_setpoint -= 256;
00336
00337 if((throttle_setpoint > 0 && *system_throttle_setpoint < 0)
00338 || (throttle_setpoint < 0 && *system_throttle_setpoint > 0))
00339 resetIntegralAndDerivative();
00340 *system_throttle_setpoint = ceilf(3.19 * (float)throttle_setpoint);
00341 }
00342 if(echo) formatSend(header, *system_throttle_setpoint);
00343 break;
00344 case 'n':
00345
00346 if(getc() == COLON) {
00347 *run_mode = (int)getc();
00348 if(*run_mode > 2) *run_mode = 2;
00349 }
00350 formatSend(header, *run_mode);
00351 break;
00352 case 'o':
00353
00354 if(getc() == COLON) {
00355 *sytem_dead_man = (int)getc();
00356 if(*sytem_dead_man != 0) *sytem_dead_man = 1;
00357 }
00358 formatSend(header, *sytem_dead_man);
00359 break;
00360 case 'p':
00361
00362 formatSend(header, getDeadMan());
00363 break;
00364 case 'q':
00365
00366 if(getc() == COLON) {
00367 echo = (int)getc();
00368 if(echo != 0) echo = 1;
00369 }
00370 formatSend(header, echo);
00371 break;
00372 default:
00373 break;
00374 }
00375 }