モータードライバとWi-FiモジュールESP-WROOM-02をmbed LPC1114FN28に繋げて、RCWControllerからコントロールするプログラム
Dependencies: mbed
main.cpp@0:3c24a40c2343, 2016-07-22 (annotated)
- Committer:
- jksoft
- Date:
- Fri Jul 22 05:36:02 2016 +0000
- Revision:
- 0:3c24a40c2343
??
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jksoft | 0:3c24a40c2343 | 1 | #include "mbed.h" |
jksoft | 0:3c24a40c2343 | 2 | |
jksoft | 0:3c24a40c2343 | 3 | #include "ESP8266Interface.h" |
jksoft | 0:3c24a40c2343 | 4 | #include "SoftSerialSendOnry.h" |
jksoft | 0:3c24a40c2343 | 5 | #include "RCWController.h" |
jksoft | 0:3c24a40c2343 | 6 | |
jksoft | 0:3c24a40c2343 | 7 | /************************* WiFi Setting *********************************/ |
jksoft | 0:3c24a40c2343 | 8 | |
jksoft | 0:3c24a40c2343 | 9 | #define WLAN_SSID "wifi_car" |
jksoft | 0:3c24a40c2343 | 10 | #define WLAN_PASS "wifi_car" |
jksoft | 0:3c24a40c2343 | 11 | |
jksoft | 0:3c24a40c2343 | 12 | |
jksoft | 0:3c24a40c2343 | 13 | /************************* UDP Setting *********************************/ |
jksoft | 0:3c24a40c2343 | 14 | #define SEND_PORT 10000 |
jksoft | 0:3c24a40c2343 | 15 | |
jksoft | 0:3c24a40c2343 | 16 | /************************* Port Class Setting *********************************/ |
jksoft | 0:3c24a40c2343 | 17 | SoftSerialSendOnry pc(dp10); // tx |
jksoft | 0:3c24a40c2343 | 18 | ESP8266Interface wifi(dp16,dp15,dp4,WLAN_SSID,WLAN_PASS); |
jksoft | 0:3c24a40c2343 | 19 | PwmOut motor_pwm_a(dp24); |
jksoft | 0:3c24a40c2343 | 20 | DigitalOut motor_sw_a(dp18); |
jksoft | 0:3c24a40c2343 | 21 | PwmOut motor_pwm_b(dp1); |
jksoft | 0:3c24a40c2343 | 22 | DigitalOut motor_sw_b(dp2); |
jksoft | 0:3c24a40c2343 | 23 | |
jksoft | 0:3c24a40c2343 | 24 | /************************* Variable Setting *********************************/ |
jksoft | 0:3c24a40c2343 | 25 | char recv_buf[10]; |
jksoft | 0:3c24a40c2343 | 26 | int recv_buf_p = 0; |
jksoft | 0:3c24a40c2343 | 27 | int read_p = 0; |
jksoft | 0:3c24a40c2343 | 28 | int recv_ana_phase = 0; |
jksoft | 0:3c24a40c2343 | 29 | |
jksoft | 0:3c24a40c2343 | 30 | /************************* Controller value to PWM vlaue *********************************/ |
jksoft | 0:3c24a40c2343 | 31 | float Controll2Pwm(int value) |
jksoft | 0:3c24a40c2343 | 32 | { |
jksoft | 0:3c24a40c2343 | 33 | return(((float)value/128.0f)-1.0f); |
jksoft | 0:3c24a40c2343 | 34 | } |
jksoft | 0:3c24a40c2343 | 35 | |
jksoft | 0:3c24a40c2343 | 36 | /************************* Main func *********************************/ |
jksoft | 0:3c24a40c2343 | 37 | int main() { |
jksoft | 0:3c24a40c2343 | 38 | char* ip; |
jksoft | 0:3c24a40c2343 | 39 | int recv_p = 0; |
jksoft | 0:3c24a40c2343 | 40 | int timeout_count = 0; |
jksoft | 0:3c24a40c2343 | 41 | |
jksoft | 0:3c24a40c2343 | 42 | pc.printf("Start\r\n"); |
jksoft | 0:3c24a40c2343 | 43 | |
jksoft | 0:3c24a40c2343 | 44 | motor_pwm_a = 0.0; |
jksoft | 0:3c24a40c2343 | 45 | motor_pwm_b = 0.0; |
jksoft | 0:3c24a40c2343 | 46 | |
jksoft | 0:3c24a40c2343 | 47 | wifi.init(); |
jksoft | 0:3c24a40c2343 | 48 | pc.printf("Wifi init OK\r\n"); |
jksoft | 0:3c24a40c2343 | 49 | |
jksoft | 0:3c24a40c2343 | 50 | wifi.single_ap(); |
jksoft | 0:3c24a40c2343 | 51 | //while(!wifi.connect()); |
jksoft | 0:3c24a40c2343 | 52 | pc.printf("Wifi Connect OK\r\n"); |
jksoft | 0:3c24a40c2343 | 53 | |
jksoft | 0:3c24a40c2343 | 54 | ip = wifi.getIPAddress(); |
jksoft | 0:3c24a40c2343 | 55 | if(ip != NULL) |
jksoft | 0:3c24a40c2343 | 56 | { |
jksoft | 0:3c24a40c2343 | 57 | pc.printf("IP:%s\r\n",ip); |
jksoft | 0:3c24a40c2343 | 58 | } |
jksoft | 0:3c24a40c2343 | 59 | else |
jksoft | 0:3c24a40c2343 | 60 | { |
jksoft | 0:3c24a40c2343 | 61 | pc.printf("IP:ERROR\r\n"); |
jksoft | 0:3c24a40c2343 | 62 | } |
jksoft | 0:3c24a40c2343 | 63 | |
jksoft | 0:3c24a40c2343 | 64 | if(wifi.start(ESP_UDP_TYPE,ip, SEND_PORT) != false ) |
jksoft | 0:3c24a40c2343 | 65 | { |
jksoft | 0:3c24a40c2343 | 66 | pc.printf("UDP Port open OK\r\n"); |
jksoft | 0:3c24a40c2343 | 67 | } |
jksoft | 0:3c24a40c2343 | 68 | else |
jksoft | 0:3c24a40c2343 | 69 | { |
jksoft | 0:3c24a40c2343 | 70 | pc.printf("UDP Port open NG\r\n"); |
jksoft | 0:3c24a40c2343 | 71 | } |
jksoft | 0:3c24a40c2343 | 72 | |
jksoft | 0:3c24a40c2343 | 73 | pc.printf("init All OK\r\n"); |
jksoft | 0:3c24a40c2343 | 74 | |
jksoft | 0:3c24a40c2343 | 75 | while(1) |
jksoft | 0:3c24a40c2343 | 76 | { |
jksoft | 0:3c24a40c2343 | 77 | while(wifi.readable()!=0) |
jksoft | 0:3c24a40c2343 | 78 | { |
jksoft | 0:3c24a40c2343 | 79 | recv_buf[recv_p] = wifi.getc(); |
jksoft | 0:3c24a40c2343 | 80 | //pc.printf("recv[%d]:\r\n",recv_buf[recv_p]); |
jksoft | 0:3c24a40c2343 | 81 | recv_p++; |
jksoft | 0:3c24a40c2343 | 82 | |
jksoft | 0:3c24a40c2343 | 83 | if(recv_p>=10) |
jksoft | 0:3c24a40c2343 | 84 | { |
jksoft | 0:3c24a40c2343 | 85 | float left,right; |
jksoft | 0:3c24a40c2343 | 86 | RCWController *controller; |
jksoft | 0:3c24a40c2343 | 87 | #if 0 |
jksoft | 0:3c24a40c2343 | 88 | pc.printf("recv[%d]:",recv_p); |
jksoft | 0:3c24a40c2343 | 89 | for(int i=0;i<recv_p;i++) |
jksoft | 0:3c24a40c2343 | 90 | { |
jksoft | 0:3c24a40c2343 | 91 | pc.printf("%02X ",recv_buf[i]); |
jksoft | 0:3c24a40c2343 | 92 | } |
jksoft | 0:3c24a40c2343 | 93 | pc.printf("\r\n"); |
jksoft | 0:3c24a40c2343 | 94 | #endif |
jksoft | 0:3c24a40c2343 | 95 | // Set Table |
jksoft | 0:3c24a40c2343 | 96 | controller = (RCWController*)&recv_buf[0]; |
jksoft | 0:3c24a40c2343 | 97 | |
jksoft | 0:3c24a40c2343 | 98 | // Change Servo Value |
jksoft | 0:3c24a40c2343 | 99 | if( (controller->status.LeftAnalogUD != 128)||(controller->status.RightAnalogUD != 128) ) |
jksoft | 0:3c24a40c2343 | 100 | { |
jksoft | 0:3c24a40c2343 | 101 | left = Controll2Pwm(controller->status.LeftAnalogUD); |
jksoft | 0:3c24a40c2343 | 102 | right = Controll2Pwm(controller->status.RightAnalogUD); |
jksoft | 0:3c24a40c2343 | 103 | |
jksoft | 0:3c24a40c2343 | 104 | |
jksoft | 0:3c24a40c2343 | 105 | } |
jksoft | 0:3c24a40c2343 | 106 | else |
jksoft | 0:3c24a40c2343 | 107 | { |
jksoft | 0:3c24a40c2343 | 108 | if( controller->status.UP == 1 ) |
jksoft | 0:3c24a40c2343 | 109 | { |
jksoft | 0:3c24a40c2343 | 110 | left = 1.0f; |
jksoft | 0:3c24a40c2343 | 111 | right = 1.0f; |
jksoft | 0:3c24a40c2343 | 112 | } |
jksoft | 0:3c24a40c2343 | 113 | else if( controller->status.DOWN == 1 ) |
jksoft | 0:3c24a40c2343 | 114 | { |
jksoft | 0:3c24a40c2343 | 115 | left = -1.0f; |
jksoft | 0:3c24a40c2343 | 116 | right = -1.0f; |
jksoft | 0:3c24a40c2343 | 117 | } |
jksoft | 0:3c24a40c2343 | 118 | else if( controller->status.RIGHT == 1 ) |
jksoft | 0:3c24a40c2343 | 119 | { |
jksoft | 0:3c24a40c2343 | 120 | left = 1.0f; |
jksoft | 0:3c24a40c2343 | 121 | right = -1.0f; |
jksoft | 0:3c24a40c2343 | 122 | } |
jksoft | 0:3c24a40c2343 | 123 | else if( controller->status.LEFT == 1 ) |
jksoft | 0:3c24a40c2343 | 124 | { |
jksoft | 0:3c24a40c2343 | 125 | left = -1.0f; |
jksoft | 0:3c24a40c2343 | 126 | right = 1.0f; |
jksoft | 0:3c24a40c2343 | 127 | } |
jksoft | 0:3c24a40c2343 | 128 | else |
jksoft | 0:3c24a40c2343 | 129 | { |
jksoft | 0:3c24a40c2343 | 130 | left = 0.0f; |
jksoft | 0:3c24a40c2343 | 131 | right = 0.0f; |
jksoft | 0:3c24a40c2343 | 132 | } |
jksoft | 0:3c24a40c2343 | 133 | } |
jksoft | 0:3c24a40c2343 | 134 | |
jksoft | 0:3c24a40c2343 | 135 | if(left > 0 ) |
jksoft | 0:3c24a40c2343 | 136 | { |
jksoft | 0:3c24a40c2343 | 137 | motor_pwm_a = left; |
jksoft | 0:3c24a40c2343 | 138 | motor_sw_a = 1; |
jksoft | 0:3c24a40c2343 | 139 | } |
jksoft | 0:3c24a40c2343 | 140 | else |
jksoft | 0:3c24a40c2343 | 141 | { |
jksoft | 0:3c24a40c2343 | 142 | motor_pwm_a = -left; |
jksoft | 0:3c24a40c2343 | 143 | motor_sw_a = 0; |
jksoft | 0:3c24a40c2343 | 144 | } |
jksoft | 0:3c24a40c2343 | 145 | if(right > 0 ) |
jksoft | 0:3c24a40c2343 | 146 | { |
jksoft | 0:3c24a40c2343 | 147 | motor_pwm_b = right; |
jksoft | 0:3c24a40c2343 | 148 | motor_sw_b = 1; |
jksoft | 0:3c24a40c2343 | 149 | } |
jksoft | 0:3c24a40c2343 | 150 | else |
jksoft | 0:3c24a40c2343 | 151 | { |
jksoft | 0:3c24a40c2343 | 152 | motor_pwm_b = -right; |
jksoft | 0:3c24a40c2343 | 153 | motor_sw_b = 0; |
jksoft | 0:3c24a40c2343 | 154 | } |
jksoft | 0:3c24a40c2343 | 155 | |
jksoft | 0:3c24a40c2343 | 156 | //pc.printf("left = %f right = %f\r\n",left,right); |
jksoft | 0:3c24a40c2343 | 157 | |
jksoft | 0:3c24a40c2343 | 158 | recv_p = 0; |
jksoft | 0:3c24a40c2343 | 159 | timeout_count = 0; |
jksoft | 0:3c24a40c2343 | 160 | } |
jksoft | 0:3c24a40c2343 | 161 | } |
jksoft | 0:3c24a40c2343 | 162 | wait(0.005); |
jksoft | 0:3c24a40c2343 | 163 | timeout_count++; |
jksoft | 0:3c24a40c2343 | 164 | if( timeout_count > 1000 ) |
jksoft | 0:3c24a40c2343 | 165 | { |
jksoft | 0:3c24a40c2343 | 166 | // Time out Reset |
jksoft | 0:3c24a40c2343 | 167 | timeout_count = 0; |
jksoft | 0:3c24a40c2343 | 168 | recv_p = 0; |
jksoft | 0:3c24a40c2343 | 169 | } |
jksoft | 0:3c24a40c2343 | 170 | } |
jksoft | 0:3c24a40c2343 | 171 | } |