Diff: main.cpp

Revision:

0:af77f3d24e4a

Child:

1:ac613232f130

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Nov 03 17:17:32 2016 +0000
@@ -0,0 +1,120 @@
+#include "mbed.h"
+#include "math.h"
+#include "Stepper.h"
+#include "MPU6050.h"
+#include "PID.h"
+#include "ESP8266Interface.h"
+#include "TCPSocketConnection.h"
+#include "TCPSocketServer.h"
+#include "Websocket.h"
+
+//------------------------------------
+// Terminal configuration
+// 9600 bauds, 8-bit data, no parity
+//------------------------------------
+//      PINOUT DESCRIPTION
+// MOTORS OUTPUT 
+// LEFT: EN-A1 STEP-A0 DIR-D13
+// RIGHT: EN-D12 STEP-D11 DIR-D10
+//
+// IMU MPU MPU6050
+// SDA-D4 SCL-D5
+//
+// DISTANCE SENSORS
+// D6, D3
+// D6 - temporary ESP8266 RESET
+//
+// ESP8266 WIFI MODULE
+// TX-D1 RX-D2 EN-D7
+//
+// WS2812B LED 
+// D2 - digital output
+// -----------------------------------
+
+//To Do
+//2 REGULATORS 
+
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+stepper left(A1,A0,D13); 
+stepper right(D12,D11,D10); 
+DigitalOut led1(LED1);
+DigitalOut wifiEnable(D7,1);
+AnalogIn shfront(D3); 
+MPU6050 imu; 
+Ticker loop;
+PID controll(120.0,0.004,0.0007,0.01);
+ESP8266Interface wifi(D1,D0,D6,"WLAN_Staszic","st@szicwifi5",115200);
+
+int i; 
+float pitch;
+float roll;
+float frontdistance; 
+double speed; 
+char str[10];
+
+
+void pid_thread() {
+    
+        imu.complementaryFilter(&pitch, &roll);
+        controll.setProcessValue(pitch); 
+        speed = controll.compute(); 
+        left.step(speed); 
+        right.step(-speed);
+}
+
+
+int main() {
+
+    left.disable(); 
+    right.disable();
+
+    imu.init();   
+    imu.calibrate(&pitch,&roll);
+    
+    //wifi.init(); //Reset
+    //wifi.connect(); //Use DHCP
+    
+    //Websocket ws("ws://192.168.0.103:1234/ws");
+    
+    //Check for successful socket connection
+    //if(!ws.connect())ws.close();
+    //else{
+        
+    left.enable(); 
+    right.enable(); //wstawaine
+    
+    //left.step(3000); 
+    //right.step(-3000);
+    wait(1);
+    wait(1); 
+    wait(1);  
+    left.step(-3000); 
+    right.step(3000);
+    wait(0.7); 
+
+    loop.attach_us(pid_thread,10000); 
+    controll.setSetPoint(-14.1);
+        
+        while(1) {
+            // string with a message
+            //sprintf(str,"%f",pitch);
+            //ws.send(str);
+        
+            //memset(str,0,10);
+            //wait(0.5);
+            //if (ws.read(str)) {
+            //    printf("rcv'd: %s\n\r", str);
+            //}
+        }
+        
+    //}
+    //ws.close();
+
+}
+
+    
+
+
+
+ 
\ No newline at end of file