Diff: main.cpp

Revision:

2:da3e288789a4

Parent:

1:0ec1b59239f2

--- a/main.cpp	Wed Dec 05 20:53:45 2018 +0000
+++ b/main.cpp	Tue Dec 11 21:22:01 2018 +0000
@@ -3,26 +3,23 @@
 
 #include "mbed.h"
 #include "PinDetect.h"
+#include "SparkfunAnalogJoystick.h"
+#include "LSM9DS1.h"
+#define PI 3.14159
 
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
+// Construct serial objects
 Serial pc(USBTX, USBRX);
 Serial esp(p28, p27); // tx, rx
-
-
 // Standard Mbed LED definitions
 DigitalOut  led1(LED1);
 DigitalOut  led2(LED2);
-DigitalOut  led3(LED3);
-DigitalOut  led4(LED4);
-
-// Push button used for testing
-PinDetect pb1(p21);
-void pushed();
-
-/*
-char ssid[32] = "hsd";     // enter WiFi router ssid inside the quotes
-char pwd [32] = "austin123"; // enter WiFi router password inside the quotes
-*/
-
+// Joystick for control
+SparkfunAnalogJoystick joystick(p18, p19, p20);
+// Construct imu object
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//sda, scl
+// function for geting mag heading
+float calc_heading(float mx, float my);
 
 // things for sending/receiving data over serial
 volatile int tx_in=0;
@@ -34,172 +31,192 @@
 char rx_buffer[buffer_size+1];
 void Tx_interrupt();
 void Rx_interrupt();
-void read_line();
+//void read_line();
 
 int DataRX;
 int update;
 char cmdbuff[1024];
 char replybuff[4096];
-char webdata[4096]; // This may need to be bigger depending on WEB browser used
+char webdata[4096];     // This may need to be bigger depending on WEB browser used
 char webbuff[4096];     // Currently using 1986 characters, Increase this if more web page data added
-void SendCMD(),getreply();
+void SendCMD();
+//void getreply();
 void getConnection();   // Sets up a connection with the car server
 char rx_line[1024];
-int port        =80;  // set server port
-int SERVtimeout =5;    // set server timeout in seconds in case link breaks.
-
-
+int port = 80;          // set server port
+int SERVtimeout = 5;    // set server timeout in seconds in case link breaks.
 
 int main()
 {
     pc.baud(9600);
     esp.baud(9600);
-    led1=1,led2=0,led3=0, led4=0;
+
+    // Init leds
+    led1=0,led2=0;
+    
     // Setup a serial interrupt function to receive data
     esp.attach(&Rx_interrupt, Serial::RxIrq);
     // Setup a serial interrupt function to transmit data
     esp.attach(&Tx_interrupt, Serial::TxIrq);
-    // Attach isr to pushbutton
-    pb1.mode(PullUp);
-    pb1.attach_asserted(&pushed);
-
+    
     // Get connection to server
     getConnection();
     
-    update = 0;
-    // Everything is interrupt driven, infinite loop
-    while(1) {
-        
-        pushed();
-        
-        //Send all car vals+
-        if (update) {
-            // Send led for testing 
-            
-            
-            // Reset update flag
-            update = 0;
+    // Set up the imu
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    IMU.calibrate(1);
+    IMU.calibrateMag(0);
+    float forward, current, diff;
+    float servo_x;
+    //initial heading calc, need to rotate by 360 degrees.
+    while(!IMU.magAvailable(X_AXIS));
+    IMU.readMag();
+    forward = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
+    
+    // Variables used for joystick
+    float lm, rm;
+    float x, y;
+    
+    while (1) {
+        // Update the motor speeds
+            // Get x and y
+            x = joystick.xAxis();
+            y = joystick.yAxis();
+            // Get the angle of direction
+        // Determine which direction (straight, complete turn, or in a coordinate quadrant)
+        // straight (set motor speed directly to y
+        if (x < .1 && x > -.1) {
+            lm = (y);
+            rm = (-1*y);\
+        }
+        // Complete turn (set outside motor directly to x)
+        else if ( y < .1 && y > -.1) {
+            if (x > 0) {
+                lm = (x);
+                rm = (0);
+            } else {
+                lm = (0);
+                rm = (x);  
+            }  
         }
         
+        // Update the servo   
+        while(!IMU.magAvailable(X_AXIS));
+        IMU.readMag();
+        current = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
+        // diff is the final difference from forward direction in [-180, 180] range
+        // this needs to     be normalized to [0, 1] while only keeping [-45, 45]
+        diff = fmod((forward - current + 180), 360) - 180;
+        diff = (diff < -180) ? (diff + 360) : diff;
+        servo_x = (diff / 90.0) + 0.45;
+        
+        // Send the updated value of the motors and servos
+        sprintf(cmdbuff, "cl:send(\"<l=%f,r=%f,x=%f>\")\r\n)", lm, rm, servo_x);
+
+        SendCMD();
+        wait(0.18);
+        led1=!led1;
     }
 }
 
-void pushed()
+// Gets magnetic heading
+float calc_heading(float mx, float my)
 {
-    strcpy(cmdbuff,"cl:send(\"flip_led1\")\r\n");
-    SendCMD();
-    //getreply();
-    wait(.1);  
+    // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
+    mx = -mx;
+    float heading;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>180.0) heading = heading - 360.0;
+    else if(heading<-180.0) heading = 360.0 + heading;
+    else if(heading<0.0) heading = 360.0  + heading;
+
+    // pc.printf("Magnetic Heading: %f degrees\n\r",heading);
+    return heading;
 }
 
+
 // Sets up connection with car server
 void getConnection()
-{
+{   
     // Reset the ESP8266
     pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
     strcpy(cmdbuff,"node.restart()\r\n");
     SendCMD();
-    wait(2);
-    getreply();
+    wait(1);
     
     // Disconncet from any potential connections
     strcpy(cmdbuff,"wifi.sta.disconnect()\r\n");
     SendCMD();
-    getreply();
-    wait(2);     
+    wait(1);     
     // Set mode (STATION)
     strcpy(cmdbuff,"wifi.setmode(wifi.STATION)\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    //getreply();
+    wait(1); 
     // Connect to car server
     strcpy(cmdbuff,"wifi.sta.config(\"Marlon's iPhone\", \"feelsbadman\")\r\n");
     SendCMD();
-    getreply();
-    wait(2);
+    wait(1);
     // Connect to server
     strcpy(cmdbuff,"wifi.sta.connect()\r\n");
     SendCMD();
-    getreply();
-    wait(2);
+    wait(1);
     
     strcpy(cmdbuff,"print(\"Looking for a connection\")\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    wait(1); 
     
     strcpy(cmdbuff,"tmr.alarm(1,2000,1, function()\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    wait(1); 
     
     strcpy(cmdbuff,"if(wifi.sta.getip()~=nil) then\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    wait(1); 
     
     strcpy(cmdbuff,"tmr.stop(1)\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    wait(1); 
     // Print out controller ip address
     strcpy(cmdbuff,"print(\"Connected!\")\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
+    wait(1); 
     // Print out controller ip address
     strcpy(cmdbuff,"print(\"Client IP Address: \",wifi.sta.getip())\r\n");
     SendCMD();
-    getreply();
-    wait(5); 
+    wait(1);
     // Create connetion
     strcpy(cmdbuff,"cl=net.createConnection(net.TCP, 0)\r\n");
     SendCMD();
-    getreply();
-    wait(2);
+    wait(1);
     // Connect the connection
     strcpy(cmdbuff,"cl:connect(80, \"192.168.4.1\")\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
-    
-//    strcpy(cmdbuff,"tmr.alarm(2, 5000, 1, function()\r\n");
-//    SendCMD();
-//    getreply();
-//    wait(2);
-//    
-//    strcpy(cmdbuff,"cl:send(\"Hello World!\")\r\n");
-//    SendCMD();
-//    getreply();
-//    wait(2);
-//    
-//    strcpy(cmdbuff,"end)\r\n");
-//    SendCMD();
-//    getreply();
-//    wait(2);
-    
-    
-    // Print out controller ip address
+    wait(1);
+   
     strcpy(cmdbuff,"else\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
-    // Print out controller ip address
+    wait(1);
+    
     strcpy(cmdbuff,"print(\"Connecting...\")\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
-    // Print out controller ip address
+    wait(1); 
+    
     strcpy(cmdbuff,"end\r\n");
     SendCMD();
-    getreply();
-    wait(2);
-    // Print out controller ip address
+    wait(1);
+    
     strcpy(cmdbuff,"end)\r\n");
     SendCMD();
-    getreply();
-    wait(2); 
-    
+    //getreply();
+    wait(5); 
 }
 
 
@@ -240,40 +257,40 @@
 
 
 // Get Command and ESP status replies
-void getreply()
-{
-    read_line();
-    sscanf(rx_line,replybuff);
-}
+//void getreply()
+//{
+//    read_line();
+//    sscanf(rx_line,replybuff);
+//}
 
 // FUNCTIONS BELOW ARE FOR RX AND TX INTERUPTS (NOT WEB STUFF)
  
 // Read a line from the large rx buffer from rx interrupt routine
-void read_line() {
-    int i;
-    i = 0;
-// Start Critical Section - don't interrupt while changing global buffer variables
-    NVIC_DisableIRQ(UART1_IRQn);
-// Loop reading rx buffer characters until end of line character
-    while ((i==0) || (rx_line[i-1] != '\r')) {
-// Wait if buffer empty
-        if (rx_in == rx_out) {
-// End Critical Section - need to allow rx interrupt to get new characters for buffer
-            NVIC_EnableIRQ(UART1_IRQn);
-            while (rx_in == rx_out) {
-            }
-// Start Critical Section - don't interrupt while changing global buffer variables
-            NVIC_DisableIRQ(UART1_IRQn);
-        }
-        rx_line[i] = rx_buffer[rx_out];
-        i++;
-        rx_out = (rx_out + 1) % buffer_size;
-    }
-// End Critical Section
-    NVIC_EnableIRQ(UART1_IRQn);
-    rx_line[i-1] = 0;
-    return;
-}
+//void read_line() {
+//    int i;
+//    i = 0;
+//// Start Critical Section - don't interrupt while changing global buffer variables
+//    NVIC_DisableIRQ(UART1_IRQn);
+//// Loop reading rx buffer characters until end of line character
+//    while ((i==0) || (rx_line[i-1] != '\r')) {
+//// Wait if buffer empty
+//        if (rx_in == rx_out) {
+//// End Critical Section - need to allow rx interrupt to get new characters for buffer
+//            NVIC_EnableIRQ(UART1_IRQn);
+//            while (rx_in == rx_out) {
+//            }
+//// Start Critical Section - don't interrupt while changing global buffer variables
+//            NVIC_DisableIRQ(UART1_IRQn);
+//        }
+//        rx_line[i] = rx_buffer[rx_out];
+//        i++;
+//        rx_out = (rx_out + 1) % buffer_size;
+//    }
+//// End Critical Section
+//    NVIC_EnableIRQ(UART1_IRQn);
+//    rx_line[i-1] = 0;
+//    return;
+//}
  
  
 // Interupt Routine to read in data from serial port
@@ -285,8 +302,8 @@
     while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
         rx_buffer[rx_in] = esp.getc();
 // Uncomment to Echo to USB serial to watch data flow
-        pc.putc(rx_buffer[rx_in]);
-        rx_in = (rx_in + 1) % buffer_size;
+//        pc.putc(rx_buffer[rx_in]);
+//        rx_in = (rx_in + 1) % buffer_size;
     }
     //led3=0;
     return;