Hang Liu / Mbed 2 deprecated HangLiu_copy

Dependencies:   ESP8266Interface MPU6050 PID StepperMotor WebSocketClient mbed

Fork of irys by Kamil Foryszewski

main.cpp@0:af77f3d24e4a, 2016-11-03 (annotated)

Committer:
kfforex
Date:
Thu Nov 03 17:17:32 2016 +0000
Revision:
0:af77f3d24e4a
Child:
1:ac613232f130
before publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kfforex 0:af77f3d24e4a 1 #include "mbed.h"
kfforex 0:af77f3d24e4a 2 #include "math.h"
kfforex 0:af77f3d24e4a 3 #include "Stepper.h"
kfforex 0:af77f3d24e4a 4 #include "MPU6050.h"
kfforex 0:af77f3d24e4a 5 #include "PID.h"
kfforex 0:af77f3d24e4a 6 #include "ESP8266Interface.h"
kfforex 0:af77f3d24e4a 7 #include "TCPSocketConnection.h"
kfforex 0:af77f3d24e4a 8 #include "TCPSocketServer.h"
kfforex 0:af77f3d24e4a 9 #include "Websocket.h"
kfforex 0:af77f3d24e4a 10
kfforex 0:af77f3d24e4a 11 //------------------------------------
kfforex 0:af77f3d24e4a 12 // Terminal configuration
kfforex 0:af77f3d24e4a 13 // 9600 bauds, 8-bit data, no parity
kfforex 0:af77f3d24e4a 14 //------------------------------------
kfforex 0:af77f3d24e4a 15 // PINOUT DESCRIPTION
kfforex 0:af77f3d24e4a 16 // MOTORS OUTPUT
kfforex 0:af77f3d24e4a 17 // LEFT: EN-A1 STEP-A0 DIR-D13
kfforex 0:af77f3d24e4a 18 // RIGHT: EN-D12 STEP-D11 DIR-D10
kfforex 0:af77f3d24e4a 19 //
kfforex 0:af77f3d24e4a 20 // IMU MPU MPU6050
kfforex 0:af77f3d24e4a 21 // SDA-D4 SCL-D5
kfforex 0:af77f3d24e4a 22 //
kfforex 0:af77f3d24e4a 23 // DISTANCE SENSORS
kfforex 0:af77f3d24e4a 24 // D6, D3
kfforex 0:af77f3d24e4a 25 // D6 - temporary ESP8266 RESET
kfforex 0:af77f3d24e4a 26 //
kfforex 0:af77f3d24e4a 27 // ESP8266 WIFI MODULE
kfforex 0:af77f3d24e4a 28 // TX-D1 RX-D2 EN-D7
kfforex 0:af77f3d24e4a 29 //
kfforex 0:af77f3d24e4a 30 // WS2812B LED
kfforex 0:af77f3d24e4a 31 // D2 - digital output
kfforex 0:af77f3d24e4a 32 // -----------------------------------
kfforex 0:af77f3d24e4a 33
kfforex 0:af77f3d24e4a 34 //To Do
kfforex 0:af77f3d24e4a 35 //2 REGULATORS
kfforex 0:af77f3d24e4a 36
kfforex 0:af77f3d24e4a 37
kfforex 0:af77f3d24e4a 38 Serial pc(SERIAL_TX, SERIAL_RX);
kfforex 0:af77f3d24e4a 39 stepper left(A1,A0,D13);
kfforex 0:af77f3d24e4a 40 stepper right(D12,D11,D10);
kfforex 0:af77f3d24e4a 41 DigitalOut led1(LED1);
kfforex 0:af77f3d24e4a 42 DigitalOut wifiEnable(D7,1);
kfforex 0:af77f3d24e4a 43 AnalogIn shfront(D3);
kfforex 0:af77f3d24e4a 44 MPU6050 imu;
kfforex 0:af77f3d24e4a 45 Ticker loop;
kfforex 0:af77f3d24e4a 46 PID controll(120.0,0.004,0.0007,0.01);
kfforex 0:af77f3d24e4a 47 ESP8266Interface wifi(D1,D0,D6,"WLAN_Staszic","st@szicwifi5",115200);
kfforex 0:af77f3d24e4a 48
kfforex 0:af77f3d24e4a 49 int i;
kfforex 0:af77f3d24e4a 50 float pitch;
kfforex 0:af77f3d24e4a 51 float roll;
kfforex 0:af77f3d24e4a 52 float frontdistance;
kfforex 0:af77f3d24e4a 53 double speed;
kfforex 0:af77f3d24e4a 54 char str[10];
kfforex 0:af77f3d24e4a 55
kfforex 0:af77f3d24e4a 56
kfforex 0:af77f3d24e4a 57 void pid_thread() {
kfforex 0:af77f3d24e4a 58
kfforex 0:af77f3d24e4a 59 imu.complementaryFilter(&pitch, &roll);
kfforex 0:af77f3d24e4a 60 controll.setProcessValue(pitch);
kfforex 0:af77f3d24e4a 61 speed = controll.compute();
kfforex 0:af77f3d24e4a 62 left.step(speed);
kfforex 0:af77f3d24e4a 63 right.step(-speed);
kfforex 0:af77f3d24e4a 64 }
kfforex 0:af77f3d24e4a 65
kfforex 0:af77f3d24e4a 66
kfforex 0:af77f3d24e4a 67 int main() {
kfforex 0:af77f3d24e4a 68
kfforex 0:af77f3d24e4a 69 left.disable();
kfforex 0:af77f3d24e4a 70 right.disable();
kfforex 0:af77f3d24e4a 71
kfforex 0:af77f3d24e4a 72 imu.init();
kfforex 0:af77f3d24e4a 73 imu.calibrate(&pitch,&roll);
kfforex 0:af77f3d24e4a 74
kfforex 0:af77f3d24e4a 75 //wifi.init(); //Reset
kfforex 0:af77f3d24e4a 76 //wifi.connect(); //Use DHCP
kfforex 0:af77f3d24e4a 77
kfforex 0:af77f3d24e4a 78 //Websocket ws("ws://192.168.0.103:1234/ws");
kfforex 0:af77f3d24e4a 79
kfforex 0:af77f3d24e4a 80 //Check for successful socket connection
kfforex 0:af77f3d24e4a 81 //if(!ws.connect())ws.close();
kfforex 0:af77f3d24e4a 82 //else{
kfforex 0:af77f3d24e4a 83
kfforex 0:af77f3d24e4a 84 left.enable();
kfforex 0:af77f3d24e4a 85 right.enable(); //wstawaine
kfforex 0:af77f3d24e4a 86
kfforex 0:af77f3d24e4a 87 //left.step(3000);
kfforex 0:af77f3d24e4a 88 //right.step(-3000);
kfforex 0:af77f3d24e4a 89 wait(1);
kfforex 0:af77f3d24e4a 90 wait(1);
kfforex 0:af77f3d24e4a 91 wait(1);
kfforex 0:af77f3d24e4a 92 left.step(-3000);
kfforex 0:af77f3d24e4a 93 right.step(3000);
kfforex 0:af77f3d24e4a 94 wait(0.7);
kfforex 0:af77f3d24e4a 95
kfforex 0:af77f3d24e4a 96 loop.attach_us(pid_thread,10000);
kfforex 0:af77f3d24e4a 97 controll.setSetPoint(-14.1);
kfforex 0:af77f3d24e4a 98
kfforex 0:af77f3d24e4a 99 while(1) {
kfforex 0:af77f3d24e4a 100 // string with a message
kfforex 0:af77f3d24e4a 101 //sprintf(str,"%f",pitch);
kfforex 0:af77f3d24e4a 102 //ws.send(str);
kfforex 0:af77f3d24e4a 103
kfforex 0:af77f3d24e4a 104 //memset(str,0,10);
kfforex 0:af77f3d24e4a 105 //wait(0.5);
kfforex 0:af77f3d24e4a 106 //if (ws.read(str)) {
kfforex 0:af77f3d24e4a 107 // printf("rcv'd: %s\n\r", str);
kfforex 0:af77f3d24e4a 108 //}
kfforex 0:af77f3d24e4a 109 }
kfforex 0:af77f3d24e4a 110
kfforex 0:af77f3d24e4a 111 //}
kfforex 0:af77f3d24e4a 112 //ws.close();
kfforex 0:af77f3d24e4a 113
kfforex 0:af77f3d24e4a 114 }
kfforex 0:af77f3d24e4a 115
kfforex 0:af77f3d24e4a 116
kfforex 0:af77f3d24e4a 117
kfforex 0:af77f3d24e4a 118
kfforex 0:af77f3d24e4a 119
kfforex 0:af77f3d24e4a 120