Diff: main.cpp

Revision:

0:ade62dde274b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Dec 10 17:45:30 2015 +0000
@@ -0,0 +1,283 @@
+/*
+#include "mbed.h"
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+
+int main()
+{
+    while (true) {
+        gpo = !gpo; // toggle pin
+        led = !led; // toggle led
+        wait(0.2f);
+    }
+}
+*/
+
+// MalletFirmware, this is the real deal version
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *      
+ *         
+ *          Pinout for FRDM-k64f                                    
+ *                                  J2
+ *                                  X X 
+ *                                  X X 
+ *                                  X X 
+ *          J3                      X X GND
+ *          X X                     X X SCLK
+ *          X X                     X X MISO
+ *          X X                     X X MOSI
+ *          X X                     X X CS
+ *          X X                 GND X X 
+ *      GND X X                     X X 
+ *      GND X X                         
+ *     5Vin X X                     J1  
+ *                                  X X 
+ *          J4                      X X motorA
+ *          X X                     X X motorB
+ *     mic1 X X                     X X 
+ *     mic2 X X                     X X 
+ *          X X                     X X 
+ *          X X               quadA X X 
+ *          X X               quadB X X Painter 1
+ *  
+ * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+ 
+#include "mbed.h"
+#include <string>
+
+//LiDAR
+#include "LidarLite.h"
+#define LIDARLite1_SDA PTC11   //SDA pin 
+#define LIDARLite1_SCL PTC10  //SCL pin 
+
+//Magnetometer
+#include "FXOS8700Q.h"
+FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1); // Proper Ports and I2C Address for K64F Freedom board
+MotionSensorDataUnits mag_data;
+MotionSensorDataCounts mag_raw;
+
+// Hardware delays
+#include "pause.cpp"
+
+// Ethernet
+#include "EthernetInterface.h"
+
+#include <stdio.h>
+#include "PeripheralNames.h"
+#include "PeripheralPins.h"
+
+// Settings for each mallet
+#define NUM         9            // this number identifies the mallet and is a value between 1-7, (8 or 9 for the painters)
+#define PORT        (55000+NUM)  // set to a random port number.  All the mallets can use the same port number.
+#define MAX_CLIENTS  2           // set the max number of clients to at least 2 (first client is MATLAB, second is the distance unit)
+#define DEBUG        0           // '1' enables debug statements printed through serial port at baud 230400, '0' disables
+
+// Ethernet
+#define LEN_PACKET 1460
+//#define N_PACKET (TOTAL_SAMPLES/LEN_PACKET)
+#define GATEWAY "169.254.99.1" // set to match your computer
+#define MASK "255.255.0.0"      // set to match your computer (probably does already)
+#define IP_LAPTOP "169.254.99.103"
+
+#define IP "169.254.99.139"
+
+// Status reporting
+
+// data good flags?
+
+// DMA status flag?
+
+// for debug purposes
+Serial pc(USBTX, USBRX);
+#if DEBUG
+#define DEBUG_PRINT(x) pc.printf(x)
+#else
+#define DEBUG_PRINT(x) do {} while(0)
+#endif
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+DigitalOut paint(D0);
+
+LidarLite sensor1(LIDARLite1_SDA, LIDARLite1_SCL); //Define LIDAR Lite sensor 1
+Timer dt;
+float distance;
+
+float fmX, fmY, fmZ;
+int16_t rmX, rmY, rmZ;
+
+char buffer[LEN_PACKET]; // general purpose tx/rx buffer
+
+// Declaration of functions
+void clearBuffer();
+
+using namespace std;
+ 
+int main() {
+    led_blue = 1;
+    led_green = 1;
+    led_red = 0;
+    
+    pc.baud(230400);
+    mag.enable();
+    dt.start();
+    
+    pc.printf("Starting M%i\r\n",NUM);
+    DEBUG_PRINT("HELLO\r\n");
+    // Give everything lower priority
+    for(int i = 0; i < 86; i++) 
+    {
+        if(NVIC_GetPriority((IRQn_Type) i) == 0) NVIC_SetPriority((IRQn_Type) i, 2);
+    }
+    
+    // Give hardware associated with 
+    // sampling the highest priority
+    NVIC_SetPriority(ADC1_IRQn,0);
+    NVIC_SetPriority(ADC0_IRQn,0);
+    NVIC_SetPriority(PDB0_IRQn,0);
+    NVIC_SetPriority(DMA0_IRQn,0);
+    NVIC_SetPriority(DMA1_IRQn,0);
+    NVIC_SetPriority(DMA2_IRQn,0);
+    
+    NVIC_SetPriority(ENET_1588_Timer_IRQn,1);
+    NVIC_SetPriority(ENET_Transmit_IRQn,1);
+    NVIC_SetPriority(ENET_Receive_IRQn,1);
+    NVIC_SetPriority(ENET_Error_IRQn,1);
+    
+    
+    // The ethernet setup must be within the first few lines of code, otherwise the program hangs
+    EthernetInterface interface;
+    interface.init(IP, MASK, GATEWAY);
+    interface.connect();
+    
+    led_green = 0;
+    
+    pc.printf("IP Address is: %s\n\r", interface.getIPAddress());
+    pc.printf("Port is: %i\n\r", PORT);
+    ENET_TIPG = 0x08; // minimum time between TCP packets
+    
+    // ethernet setup failed for some reason.  Flash yellow light then uC resets itself
+    if(interface.getIPAddress() == 0)
+    {
+        for(int i = 0; i < 5; i++)
+        {
+            // flash yellow LED
+            led_red = 0;
+            led_green = 0;
+            pause_ms(500);
+            led_red = 1;
+            led_green = 1;
+            pause_ms(1000);
+        }
+        NVIC_SystemReset();
+    }
+
+    
+    TCPSocketServer server;
+    server.bind(PORT);
+    server.listen(MAX_CLIENTS);
+        
+    led_green = 1;
+    led_red = 1;
+    led_blue = 1;
+    pc.printf("Server started\r\n");
+   
+    while(true) { // tcp connect/disconnect loop
+        
+        TCPSocketConnection laptop;
+        server.accept(laptop);
+        laptop.set_blocking(false, 9000); // timeout after 9s
+        
+        bool ipVerified = true;
+        string ipAddress = laptop.get_address();
+        if(ipAddress != IP_LAPTOP) ipVerified = false;
+        int n;
+        while(true) { // rx/tx loop
+            n = laptop.receive(buffer,LEN_PACKET); // this operation times out after 1.5s
+            if(n <= 0) break; // exit rx/tx loop
+            
+            if(!ipVerified){
+                clearBuffer();
+                sprintf(buffer,"Incorrect IP address");
+                n = laptop.send(buffer,LEN_PACKET);
+                pc.printf("%s\r\n");
+                break; // exit rx/tx loop
+            }
+            
+            if(buffer[0] == ':' && ipVerified) {
+                /*
+                if(n == 2){
+                    if(buffer[1] == '.'){
+                        paint = 1;
+                        pause_ms(100);
+                        paint = 0;
+                    }
+                    else if(buffer[1] == '-'){
+                        paint = 1;
+                        pause_ms(300);
+                        paint = 0;
+                    }
+                }
+                */
+                
+                //dt.start();
+          
+                /*
+                pc.printf("range: %d cm, velocity: %d cm/s, rate: %.2f Hz\n", sensor1.getRange_cm(), sensor1.getVelocity_cms(), 1/dt.read());
+                dt.reset();
+                */
+                
+                if(n == 2){
+                    if(buffer[1] == 'l'){
+                        sensor1.refreshRangeVelocity();
+                        //distance = double(sensor1.getRange_cm())*.393701;
+                        //pc.printf("Distance: %f in\r\n", float(sensor1.getRange_cm())*.393701);
+                        clearBuffer();    
+                        sprintf(buffer, "Distance: %f inches", float(sensor1.getRange_cm())*.393701);
+                        n = laptop.send(buffer,LEN_PACKET);
+                    }
+                    else if(buffer[1] == 'm'){
+                        clearBuffer();
+                        
+                        mag.getAxis(mag_data);
+                        //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
+                        
+                        mag.getX(&fmX);
+                        mag.getY(&fmY);
+                        mag.getZ(&fmZ);
+                        //sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
+                        
+                        mag.getAxis(mag_raw);
+                        //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
+                        
+                        mag.getX(&rmX);
+                        mag.getY(&rmY);
+                        mag.getZ(&rmZ);
+                        //sprintf(buffer, "MAG: X=%d Y=%d Z=%d\r\n", rmX, rmY, rmZ);
+                        
+                        sprintf(buffer, "MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\nMAG: X=%4.1f Y=%4.1f Z=%4.1f\r\nMAG: X=%d Y=%d Z=%d\r\nMAG: X=%d Y=%d Z=%d\r\n\n", mag_data.x, mag_data.y, mag_data.z, fmX, fmY, fmZ, mag_raw.x, mag_raw.y, mag_raw.z, rmX, rmY, rmZ);
+                        n = laptop.send(buffer,LEN_PACKET);
+                    }
+                    else{
+                        clearBuffer();     
+                        sprintf(buffer, "Invalid Input");
+                        n = laptop.send(buffer,LEN_PACKET);
+                    }
+                }
+                else{
+                    clearBuffer();     
+                    sprintf(buffer, "Invalid Input");
+                    n = laptop.send(buffer,LEN_PACKET); 
+                }
+                //pc.printf("%s\r\n",buffer); 
+                
+            }           // end if(buffer[0] == ':')
+            if(n <= 0) break;
+        }               // end while(true) rx/tx loop
+    }                   // end while(true) tcp connect/disconnect loop
+}                       // end main
+
+void clearBuffer() {
+    for(int i = 0; i < 100; i++) buffer[i] = 0;
+}
+