Hang Liu
Dependencies: ESP8266Interface MPU6050 PID StepperMotor WebSocketClient mbed
Fork of
irys
by 
Kamil Foryszewski
main.cpp@1:ac613232f130, 2016-11-08 (annotated)
- Committer:
- HangL
- Date:
- Tue Nov 08 21:19:59 2016 +0000
- Revision:
- 1:ac613232f130
- Parent:
- 0:af77f3d24e4a
Who changed what in which revision?
| User | Revision | Line number | New 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; |
| HangL | 1:ac613232f130 | 46 | //PID controll(120.0,0.004,0.0007,0.01); |
| HangL | 1:ac613232f130 | 47 | PID controll(0.01); |
| kfforex | 0:af77f3d24e4a | 48 | ESP8266Interface wifi(D1,D0,D6,"WLAN_Staszic","st@szicwifi5",115200); |
| kfforex | 0:af77f3d24e4a | 49 | |
| kfforex | 0:af77f3d24e4a | 50 | int i; |
| kfforex | 0:af77f3d24e4a | 51 | float pitch; |
| kfforex | 0:af77f3d24e4a | 52 | float roll; |
| kfforex | 0:af77f3d24e4a | 53 | float frontdistance; |
| kfforex | 0:af77f3d24e4a | 54 | double speed; |
| HangL | 1:ac613232f130 | 55 | double Stepper_speed; |
| HangL | 1:ac613232f130 | 56 | double Target_speed=0;//6.0(it is a good paraments with 6.0!! very good! nearly to be static !);//0.0; |
| kfforex | 0:af77f3d24e4a | 57 | char str[10]; |
| kfforex | 0:af77f3d24e4a | 58 | |
| kfforex | 0:af77f3d24e4a | 59 | |
| kfforex | 0:af77f3d24e4a | 60 | void pid_thread() { |
| kfforex | 0:af77f3d24e4a | 61 | |
| kfforex | 0:af77f3d24e4a | 62 | imu.complementaryFilter(&pitch, &roll); |
| kfforex | 0:af77f3d24e4a | 63 | controll.setProcessValue(pitch); |
| kfforex | 0:af77f3d24e4a | 64 | speed = controll.compute(); |
| HangL | 1:ac613232f130 | 65 | // Stepper_speed=controll.Speedcompute(speed,Target_speed); |
| kfforex | 0:af77f3d24e4a | 66 | left.step(speed); |
| kfforex | 0:af77f3d24e4a | 67 | right.step(-speed); |
| HangL | 1:ac613232f130 | 68 | |
| HangL | 1:ac613232f130 | 69 | Stepper_speed=controll.Speedcompute(speed,Target_speed); |
| HangL | 1:ac613232f130 | 70 | left.step(Stepper_speed); |
| HangL | 1:ac613232f130 | 71 | right.step(-Stepper_speed); |
| kfforex | 0:af77f3d24e4a | 72 | } |
| kfforex | 0:af77f3d24e4a | 73 | |
| kfforex | 0:af77f3d24e4a | 74 | |
| kfforex | 0:af77f3d24e4a | 75 | int main() { |
| kfforex | 0:af77f3d24e4a | 76 | |
| kfforex | 0:af77f3d24e4a | 77 | left.disable(); |
| kfforex | 0:af77f3d24e4a | 78 | right.disable(); |
| kfforex | 0:af77f3d24e4a | 79 | |
| kfforex | 0:af77f3d24e4a | 80 | imu.init(); |
| kfforex | 0:af77f3d24e4a | 81 | imu.calibrate(&pitch,&roll); |
| kfforex | 0:af77f3d24e4a | 82 | |
| kfforex | 0:af77f3d24e4a | 83 | //wifi.init(); //Reset |
| kfforex | 0:af77f3d24e4a | 84 | //wifi.connect(); //Use DHCP |
| kfforex | 0:af77f3d24e4a | 85 | |
| kfforex | 0:af77f3d24e4a | 86 | //Websocket ws("ws://192.168.0.103:1234/ws"); |
| kfforex | 0:af77f3d24e4a | 87 | |
| kfforex | 0:af77f3d24e4a | 88 | //Check for successful socket connection |
| kfforex | 0:af77f3d24e4a | 89 | //if(!ws.connect())ws.close(); |
| kfforex | 0:af77f3d24e4a | 90 | //else{ |
| kfforex | 0:af77f3d24e4a | 91 | |
| kfforex | 0:af77f3d24e4a | 92 | left.enable(); |
| kfforex | 0:af77f3d24e4a | 93 | right.enable(); //wstawaine |
| kfforex | 0:af77f3d24e4a | 94 | |
| kfforex | 0:af77f3d24e4a | 95 | //left.step(3000); |
| kfforex | 0:af77f3d24e4a | 96 | //right.step(-3000); |
| kfforex | 0:af77f3d24e4a | 97 | wait(1); |
| kfforex | 0:af77f3d24e4a | 98 | wait(1); |
| kfforex | 0:af77f3d24e4a | 99 | wait(1); |
| kfforex | 0:af77f3d24e4a | 100 | left.step(-3000); |
| kfforex | 0:af77f3d24e4a | 101 | right.step(3000); |
| kfforex | 0:af77f3d24e4a | 102 | wait(0.7); |
| kfforex | 0:af77f3d24e4a | 103 | |
| kfforex | 0:af77f3d24e4a | 104 | loop.attach_us(pid_thread,10000); |
| kfforex | 0:af77f3d24e4a | 105 | controll.setSetPoint(-14.1); |
| kfforex | 0:af77f3d24e4a | 106 | while(1) { |
| kfforex | 0:af77f3d24e4a | 107 | // string with a message |
| kfforex | 0:af77f3d24e4a | 108 | //sprintf(str,"%f",pitch); |
| kfforex | 0:af77f3d24e4a | 109 | //ws.send(str); |
| kfforex | 0:af77f3d24e4a | 110 | |
| kfforex | 0:af77f3d24e4a | 111 | //memset(str,0,10); |
| kfforex | 0:af77f3d24e4a | 112 | //wait(0.5); |
| kfforex | 0:af77f3d24e4a | 113 | //if (ws.read(str)) { |
| kfforex | 0:af77f3d24e4a | 114 | // printf("rcv'd: %s\n\r", str); |
| kfforex | 0:af77f3d24e4a | 115 | //} |
| kfforex | 0:af77f3d24e4a | 116 | } |
| kfforex | 0:af77f3d24e4a | 117 | |
| kfforex | 0:af77f3d24e4a | 118 | //} |
| kfforex | 0:af77f3d24e4a | 119 | //ws.close(); |
| kfforex | 0:af77f3d24e4a | 120 | |
| kfforex | 0:af77f3d24e4a | 121 | } |
| kfforex | 0:af77f3d24e4a | 122 | |
| kfforex | 0:af77f3d24e4a | 123 | |
| kfforex | 0:af77f3d24e4a | 124 | |
| kfforex | 0:af77f3d24e4a | 125 | |
| kfforex | 0:af77f3d24e4a | 126 | |
| kfforex | 0:af77f3d24e4a | 127 |