main.cpp

00001 #include "mbed.h"
00002 #include "tsi_sensor.h"
00003 
00004 #include "main.h"
00005 
00006 // Comunicación mediante XBee
00007 // Serial pc(PTE0, PTE1);
00008 
00009 // Comunicación mediante Serial
00010 Serial pc(USBTX, USBRX);
00011 
00012 // Comunicacion al GPS
00013 Serial gps(PTE22, PTE23);
00014 
00015 // ------ Configuracion de la maquina de toques ------
00016 DigitalOut taser(PTE5);
00017 
00018 InterruptIn emergency_stop(PTC7);
00019 DigitalIn enter_button(PTC0);
00020 
00021 float vel_setpoint = 0;
00022 float time_setpoint = 0;
00023 
00024 // ----------------------------------------------------
00025 
00026 
00027 // ------ Configuracion de LEDs ------
00028 DigitalOut slow_led(LED1);
00029 DigitalOut ok_led(LED2);
00030 DigitalOut booting_led(LED3);
00031 
00032 Ticker gps_status_ticker;
00033 bool status_set = false;
00034 bool led_tick = false;
00035 // -----------------------------------
00036 
00037 // definiciones para la lectura del gps
00038 char buffer[256];
00039 char gprmc_buffer[256];
00040 
00041 char type[7];
00042 
00043 bool readline = false;
00044 int i = 0;
00045 
00046 GPRMC_t data;
00047 // fin de las definiciones
00048 
00049 int main() {
00050     gps.attach(&rcv_callback, Serial::RxIrq);
00051     
00052     enter_button.mode(PullUp);
00053     emergency_stop.rise(&stop_all);
00054     
00055     slow_led = 1;
00056     ok_led = 1;
00057     booting_led = 1;
00058     
00059     pc.printf("Ing. Vel:\n");
00060     vel_setpoint = get_setpoint(enter_button, "%.3f km/h\n");
00061     
00062     pc.printf("Ing. Tiempo:\n");
00063     time_setpoint = get_setpoint(enter_button, "%.3f mins\n");
00064     
00065     while(1) {
00066         if (readline) {
00067             readline = false;
00068             i = 0;
00069                 
00070             memcpy(type, &buffer[0], 6);
00071             type[6] = 0;
00072                 
00073                 
00074             if (strcmp(type, "$GPRMC") == 0) {
00075                 strcpy(gprmc_buffer, buffer);
00076                 
00077                 data = get_gprmc(gprmc_buffer);
00078                 print_gprmc(data);
00079                 
00080                 if (data.valid) {
00081                     // Se apaga el LED de búsqueda
00082                     
00083                     if (status_set) {
00084                         booting_led = 1;
00085                         gps_status_ticker.detach();
00086                         status_set = false;
00087                     }
00088                     
00089                     if (data.vel < vel_setpoint) {
00090                         ok_led = 1;
00091                         slow_led = 0;
00092                         
00093                         taser = 1;
00094                     }
00095                     else {
00096                         ok_led = 0;
00097                         slow_led = 1;
00098                         
00099                         taser = 0;
00100                     }
00101                 }
00102                 else if (!status_set && !data.valid) {
00103                     // Se está buscando la señal de GPS
00104                     booting_led = 1;
00105                     gps_status_ticker.attach(&finding_gps, 2.5);
00106                     pc.printf("Ticker set...\n");
00107                     
00108                     status_set = true;
00109                     
00110                     ok_led = 1;
00111                     slow_led = 1;
00112                 }
00113             }
00114         }
00115     }
00116 }
00117 
00118 void rcv_callback() {
00119     char c;
00120     
00121     while (gps.readable()) {
00122         c = gps.getc();
00123         
00124         if (c != '\n') {
00125             buffer[i] = c;
00126         }
00127         else {
00128             readline = true;
00129             buffer[i] = 0;
00130         }
00131             
00132         i++;
00133     }
00134 }
00135 
00136 void finding_gps() {
00137     pc.printf("tick tock\n");
00138     
00139     if (led_tick) {
00140         booting_led = 1;
00141         led_tick = false;
00142     }
00143     else {
00144         booting_led = 0;
00145         led_tick = true;
00146     }
00147 }
00148 
00149 GPRMC_t get_gprmc(char *line) {
00150     char *token;
00151     int token_index = 0;
00152     
00153     GPRMC_t gprmc;
00154     gprmc.vel = -1;
00155     gprmc.valid = false;
00156     
00157     pc.printf("%s\n", line);
00158     token = strtok(line, ",");
00159     
00160     while (token != NULL) {        
00161         if (token_index == 2) {
00162             if (strcmp(token, "A") == 0) {
00163                 gprmc.valid = true;
00164             }
00165         }
00166         
00167         if (gprmc.valid) {
00168             switch (token_index) {
00169                 case 3:
00170                     // lat
00171                     strcpy(gprmc.lat, token);
00172                     break;
00173                 case 4:
00174                     // NS
00175                     strcpy(gprmc.ns, token);
00176                     break;
00177                     
00178                 case 5:
00179                     // lon
00180                     strcpy(gprmc.lon, token);
00181                     break;
00182                 case 6:
00183                     // EW
00184                     strcpy(gprmc.ew, token);
00185                     break;
00186                     
00187                 case 7:
00188                     gprmc.vel = atof(token) * 1.852;
00189                     break;
00190                     
00191                 default:
00192                     break;
00193             }
00194         }
00195         
00196         token = strtok(NULL, ",");
00197         token_index += 1;
00198     }
00199     
00200     pc.printf("\n");
00201     
00202     return gprmc;
00203 }
00204 
00205 void print_gprmc(GPRMC_t packet) {
00206     pc.printf("Valid: %d\n", packet.valid);
00207     
00208     if (packet.valid) {
00209         pc.printf("Lat: %s %s\n", packet.lat, packet.ns);
00210         pc.printf("Lon: %s %s\n", packet.lon, packet.ew);
00211         pc.printf("Spd: %.2f\n\n", packet.vel);
00212     }
00213 }
00214 
00215 float get_setpoint(DigitalIn enter_button, char msg[]) {
00216     TSIAnalogSlider tsi(PTB16, PTB17, 40);
00217     
00218     float current_tsi = 0;
00219     float prev_tsi = 0;
00220     float dt_tsi = 0;
00221     
00222     float rolling = 0;
00223     
00224     float setpoint = 0;
00225     float dt_setpoint = 0.5;
00226 
00227     while (enter_button.read() != 0) {
00228         // velocity setpoint
00229         prev_tsi = current_tsi;
00230         current_tsi = tsi.readPercentage();
00231         
00232         rolling = 0.1 * rolling + current_tsi;
00233         dt_tsi = prev_tsi - rolling;
00234         
00235         if (prev_tsi > 0 && current_tsi > 0) {
00236             if (dt_tsi > 0.01) {
00237                 if (setpoint + dt_setpoint <= 10) {
00238                     setpoint += dt_setpoint;
00239                 }
00240             } 
00241             
00242             if (dt_tsi < -0.1) {
00243                 if (setpoint - dt_setpoint >= 0) {
00244                     setpoint -= dt_setpoint;
00245                 }
00246             }
00247             
00248             // cambiar a display
00249             pc.printf(msg, setpoint);
00250         }
00251         
00252         wait(0.05);
00253     }
00254     
00255     return setpoint;
00256 }
00257 
00258 void send_gps_data() {
00259     if (data.valid) {
00260         pc.printf("{\"lat\": \"%s%s\", \"lon\": \"%s%s\", \"spd\": %.3f}\n", data.lat, data.ns, data.lon, data.ew, data.vel);
00261     }
00262 }
00263 
00264 void stop_all() {
00265     taser = 0;
00266     
00267     // Cancela todo
00268     while (1) {}
00269 }