Diff: main.cpp

Revision:

0:0d288018309d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 02 16:35:54 2019 +0000
@@ -0,0 +1,269 @@
+#include "mbed.h"
+#include "tsi_sensor.h"
+
+#include "main.h"
+
+// Comunicación mediante XBee
+// Serial pc(PTE0, PTE1);
+
+// Comunicación mediante Serial
+Serial pc(USBTX, USBRX);
+
+// Comunicacion al GPS
+Serial gps(PTE22, PTE23);
+
+// ------ Configuracion de la maquina de toques ------
+DigitalOut taser(PTE5);
+
+InterruptIn emergency_stop(PTC7);
+DigitalIn enter_button(PTC0);
+
+float vel_setpoint = 0;
+float time_setpoint = 0;
+
+// ----------------------------------------------------
+
+
+// ------ Configuracion de LEDs ------
+DigitalOut slow_led(LED1);
+DigitalOut ok_led(LED2);
+DigitalOut booting_led(LED3);
+
+Ticker gps_status_ticker;
+bool status_set = false;
+bool led_tick = false;
+// -----------------------------------
+
+// definiciones para la lectura del gps
+char buffer[256];
+char gprmc_buffer[256];
+
+char type[7];
+
+bool readline = false;
+int i = 0;
+
+GPRMC_t data;
+// fin de las definiciones
+
+int main() {
+    gps.attach(&rcv_callback, Serial::RxIrq);
+    
+    enter_button.mode(PullUp);
+    emergency_stop.rise(&stop_all);
+    
+    slow_led = 1;
+    ok_led = 1;
+    booting_led = 1;
+    
+    pc.printf("Ing. Vel:\n");
+    vel_setpoint = get_setpoint(enter_button, "%.3f km/h\n");
+    
+    pc.printf("Ing. Tiempo:\n");
+    time_setpoint = get_setpoint(enter_button, "%.3f mins\n");
+    
+    while(1) {
+        if (readline) {
+            readline = false;
+            i = 0;
+                
+            memcpy(type, &buffer[0], 6);
+            type[6] = 0;
+                
+                
+            if (strcmp(type, "$GPRMC") == 0) {
+                strcpy(gprmc_buffer, buffer);
+                
+                data = get_gprmc(gprmc_buffer);
+                print_gprmc(data);
+                
+                if (data.valid) {
+                    // Se apaga el LED de búsqueda
+                    
+                    if (status_set) {
+                        booting_led = 1;
+                        gps_status_ticker.detach();
+                        status_set = false;
+                    }
+                    
+                    if (data.vel < vel_setpoint) {
+                        ok_led = 1;
+                        slow_led = 0;
+                        
+                        taser = 1;
+                    }
+                    else {
+                        ok_led = 0;
+                        slow_led = 1;
+                        
+                        taser = 0;
+                    }
+                }
+                else if (!status_set && !data.valid) {
+                    // Se está buscando la señal de GPS
+                    booting_led = 1;
+                    gps_status_ticker.attach(&finding_gps, 2.5);
+                    pc.printf("Ticker set...\n");
+                    
+                    status_set = true;
+                    
+                    ok_led = 1;
+                    slow_led = 1;
+                }
+            }
+        }
+    }
+}
+
+void rcv_callback() {
+    char c;
+    
+    while (gps.readable()) {
+        c = gps.getc();
+        
+        if (c != '\n') {
+            buffer[i] = c;
+        }
+        else {
+            readline = true;
+            buffer[i] = 0;
+        }
+            
+        i++;
+    }
+}
+
+void finding_gps() {
+    pc.printf("tick tock\n");
+    
+    if (led_tick) {
+        booting_led = 1;
+        led_tick = false;
+    }
+    else {
+        booting_led = 0;
+        led_tick = true;
+    }
+}
+
+GPRMC_t get_gprmc(char *line) {
+    char *token;
+    int token_index = 0;
+    
+    GPRMC_t gprmc;
+    gprmc.vel = -1;
+    gprmc.valid = false;
+    
+    pc.printf("%s\n", line);
+    token = strtok(line, ",");
+    
+    while (token != NULL) {        
+        if (token_index == 2) {
+            if (strcmp(token, "A") == 0) {
+                gprmc.valid = true;
+            }
+        }
+        
+        if (gprmc.valid) {
+            switch (token_index) {
+                case 3:
+                    // lat
+                    strcpy(gprmc.lat, token);
+                    break;
+                case 4:
+                    // NS
+                    strcpy(gprmc.ns, token);
+                    break;
+                    
+                case 5:
+                    // lon
+                    strcpy(gprmc.lon, token);
+                    break;
+                case 6:
+                    // EW
+                    strcpy(gprmc.ew, token);
+                    break;
+                    
+                case 7:
+                    gprmc.vel = atof(token) * 1.852;
+                    break;
+                    
+                default:
+                    break;
+            }
+        }
+        
+        token = strtok(NULL, ",");
+        token_index += 1;
+    }
+    
+    pc.printf("\n");
+    
+    return gprmc;
+}
+
+void print_gprmc(GPRMC_t packet) {
+    pc.printf("Valid: %d\n", packet.valid);
+    
+    if (packet.valid) {
+        pc.printf("Lat: %s %s\n", packet.lat, packet.ns);
+        pc.printf("Lon: %s %s\n", packet.lon, packet.ew);
+        pc.printf("Spd: %.2f\n\n", packet.vel);
+    }
+}
+
+float get_setpoint(DigitalIn enter_button, char msg[]) {
+    TSIAnalogSlider tsi(PTB16, PTB17, 40);
+    
+    float current_tsi = 0;
+    float prev_tsi = 0;
+    float dt_tsi = 0;
+    
+    float rolling = 0;
+    
+    float setpoint = 0;
+    float dt_setpoint = 0.5;
+
+    while (enter_button.read() != 0) {
+        // velocity setpoint
+        prev_tsi = current_tsi;
+        current_tsi = tsi.readPercentage();
+        
+        rolling = 0.1 * rolling + current_tsi;
+        dt_tsi = prev_tsi - rolling;
+        
+        if (prev_tsi > 0 && current_tsi > 0) {
+            if (dt_tsi > 0.01) {
+                if (setpoint + dt_setpoint <= 10) {
+                    setpoint += dt_setpoint;
+                }
+            } 
+            
+            if (dt_tsi < -0.1) {
+                if (setpoint - dt_setpoint >= 0) {
+                    setpoint -= dt_setpoint;
+                }
+            }
+            
+            // cambiar a display
+            pc.printf(msg, setpoint);
+        }
+        
+        wait(0.05);
+    }
+    
+    return setpoint;
+}
+
+void send_gps_data() {
+    if (data.valid) {
+        pc.printf("{\"lat\": \"%s%s\", \"lon\": \"%s%s\", \"spd\": %.3f}\n", data.lat, data.ns, data.lon, data.ew, data.vel);
+    }
+}
+
+void stop_all() {
+    taser = 0;
+    
+    // Cancela todo
+    while (1) {}
+}
\ No newline at end of file