Electric Skateboard using Wi-Fi controller.
Dependencies: Servo WizFi250Interface mbed
main.cpp@0:aca1f2f5443d, 2015-10-28 (annotated)
- Committer:
- kaizen
- Date:
- Wed Oct 28 11:25:26 2015 +0000
- Revision:
- 0:aca1f2f5443d
- Child:
- 1:a710607c27cd
First Demo version of Wifi skateboard
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kaizen | 0:aca1f2f5443d | 1 | #include "mbed.h" |
kaizen | 0:aca1f2f5443d | 2 | #include "Servo.h" |
kaizen | 0:aca1f2f5443d | 3 | #include "WizFi250Interface.h" |
kaizen | 0:aca1f2f5443d | 4 | |
kaizen | 0:aca1f2f5443d | 5 | #define SECURE WizFi250::SEC_WPA2_MIXED |
kaizen | 0:aca1f2f5443d | 6 | #define SSID "Kaizen_Skate" |
kaizen | 0:aca1f2f5443d | 7 | #define PASS "qwertyuiop" |
kaizen | 0:aca1f2f5443d | 8 | |
kaizen | 0:aca1f2f5443d | 9 | #define SERVER_PORT 5000 |
kaizen | 0:aca1f2f5443d | 10 | #define STOP_MOTOR 50 |
kaizen | 0:aca1f2f5443d | 11 | |
kaizen | 0:aca1f2f5443d | 12 | |
kaizen | 0:aca1f2f5443d | 13 | #if defined(TARGET_WIZwiki_W7500P) |
kaizen | 0:aca1f2f5443d | 14 | WizFi250Interface wizfi250(PC_2,PC_3,PC_13,PC_14,PC_12,NC,115200); |
kaizen | 0:aca1f2f5443d | 15 | Serial pc(USBTX, USBRX); |
kaizen | 0:aca1f2f5443d | 16 | Servo motor_speed(PC_8); // it used to percentage value ( 0 ~ 1 ) |
kaizen | 0:aca1f2f5443d | 17 | DigitalOut green_led(LED2); |
kaizen | 0:aca1f2f5443d | 18 | DigitalOut red_led(LED1); |
kaizen | 0:aca1f2f5443d | 19 | DigitalOut blue_led(LED3); |
kaizen | 0:aca1f2f5443d | 20 | #endif |
kaizen | 0:aca1f2f5443d | 21 | |
kaizen | 0:aca1f2f5443d | 22 | //#define START_SIGNAL 0x0102FEFD |
kaizen | 0:aca1f2f5443d | 23 | //#define END_SIGNAL 0x0201FDFE |
kaizen | 0:aca1f2f5443d | 24 | //#define CONTROL_UP 0x0304FCFB |
kaizen | 0:aca1f2f5443d | 25 | //#define CONTROL_DOWN 0x0403FBFD |
kaizen | 0:aca1f2f5443d | 26 | //#define CONTROL_CRUISER 0x0506FAF9 |
kaizen | 0:aca1f2f5443d | 27 | // |
kaizen | 0:aca1f2f5443d | 28 | //#define STATUS_CMD_OK 0x10101010 |
kaizen | 0:aca1f2f5443d | 29 | //#define STATUS_CMD_FAIL 0x01010101 |
kaizen | 0:aca1f2f5443d | 30 | |
kaizen | 0:aca1f2f5443d | 31 | #define START_MSG "START" |
kaizen | 0:aca1f2f5443d | 32 | #define END_MSG "END\n" |
kaizen | 0:aca1f2f5443d | 33 | #define CONTROL_UP "UPXX" |
kaizen | 0:aca1f2f5443d | 34 | #define CONTROL_DOWN "DOWN" |
kaizen | 0:aca1f2f5443d | 35 | #define CONTROL_CRUISER "CRUI" |
kaizen | 0:aca1f2f5443d | 36 | #define CONTROL_STOP "STOP" |
kaizen | 0:aca1f2f5443d | 37 | |
kaizen | 0:aca1f2f5443d | 38 | #define STATUS_CMD_OK "OKXX" |
kaizen | 0:aca1f2f5443d | 39 | #define STATUS_CMD_FAIL "FAIL" |
kaizen | 0:aca1f2f5443d | 40 | |
kaizen | 0:aca1f2f5443d | 41 | #define CONTROL_MSG_SIZE 19 |
kaizen | 0:aca1f2f5443d | 42 | |
kaizen | 0:aca1f2f5443d | 43 | // Control Pkt Format : START,CONTROL,SPEED_VALUE,END |
kaizen | 0:aca1f2f5443d | 44 | // START,UPXX,500,END |
kaizen | 0:aca1f2f5443d | 45 | // Status Pkt Format : START,CONTROL,SPEED_VALUE,STATUS,END |
kaizen | 0:aca1f2f5443d | 46 | |
kaizen | 0:aca1f2f5443d | 47 | struct control_pkt{ |
kaizen | 0:aca1f2f5443d | 48 | char start_msg[6]; |
kaizen | 0:aca1f2f5443d | 49 | char control_msg[5]; |
kaizen | 0:aca1f2f5443d | 50 | char speed_msg[4]; |
kaizen | 0:aca1f2f5443d | 51 | char end_msg[4]; |
kaizen | 0:aca1f2f5443d | 52 | }; |
kaizen | 0:aca1f2f5443d | 53 | void parse(char buffer[], int *j, char *string); |
kaizen | 0:aca1f2f5443d | 54 | //struct status_pkt{ |
kaizen | 0:aca1f2f5443d | 55 | // char start_msg[6]; |
kaizen | 0:aca1f2f5443d | 56 | // char speed_msg[4]; |
kaizen | 0:aca1f2f5443d | 57 | // char status_msg[5]; |
kaizen | 0:aca1f2f5443d | 58 | // char end_msg[4]; |
kaizen | 0:aca1f2f5443d | 59 | //} |
kaizen | 0:aca1f2f5443d | 60 | |
kaizen | 0:aca1f2f5443d | 61 | int main() { |
kaizen | 0:aca1f2f5443d | 62 | int recv_cnt,j; |
kaizen | 0:aca1f2f5443d | 63 | volatile float speed_value = 0.5; |
kaizen | 0:aca1f2f5443d | 64 | char recv_control_msg[100]; |
kaizen | 0:aca1f2f5443d | 65 | char status_msg[100]; |
kaizen | 0:aca1f2f5443d | 66 | control_pkt control; |
kaizen | 0:aca1f2f5443d | 67 | |
kaizen | 0:aca1f2f5443d | 68 | pc.baud(115200); |
kaizen | 0:aca1f2f5443d | 69 | green_led = 1; red_led = 1; blue_led = 1; |
kaizen | 0:aca1f2f5443d | 70 | |
kaizen | 0:aca1f2f5443d | 71 | wizfi250.init(); |
kaizen | 0:aca1f2f5443d | 72 | wizfi250.setAntMode(WizFi250::PCB); |
kaizen | 0:aca1f2f5443d | 73 | wizfi250.setAddress("192.168.100.2","255.255.255.0","192.168.100.1"); |
kaizen | 0:aca1f2f5443d | 74 | if ( wizfi250.connect(SECURE, SSID, PASS, WizFi250::WM_AP)) |
kaizen | 0:aca1f2f5443d | 75 | { |
kaizen | 0:aca1f2f5443d | 76 | red_led = 0; |
kaizen | 0:aca1f2f5443d | 77 | return -1; |
kaizen | 0:aca1f2f5443d | 78 | } |
kaizen | 0:aca1f2f5443d | 79 | printf("IP Address is %s\r\n", wizfi250.getIPAddress()); |
kaizen | 0:aca1f2f5443d | 80 | green_led = 0; red_led = 0; blue_led = 0; |
kaizen | 0:aca1f2f5443d | 81 | |
kaizen | 0:aca1f2f5443d | 82 | TCPSocketServer server; |
kaizen | 0:aca1f2f5443d | 83 | TCPSocketConnection client; |
kaizen | 0:aca1f2f5443d | 84 | |
kaizen | 0:aca1f2f5443d | 85 | while(true) |
kaizen | 0:aca1f2f5443d | 86 | { |
kaizen | 0:aca1f2f5443d | 87 | if( client.is_connected() == false ) |
kaizen | 0:aca1f2f5443d | 88 | { |
kaizen | 0:aca1f2f5443d | 89 | green_led = 1; red_led = 1; blue_led = 0; |
kaizen | 0:aca1f2f5443d | 90 | client.close(); |
kaizen | 0:aca1f2f5443d | 91 | server.close(); |
kaizen | 0:aca1f2f5443d | 92 | |
kaizen | 0:aca1f2f5443d | 93 | if(speed_value == 0.5) |
kaizen | 0:aca1f2f5443d | 94 | { |
kaizen | 0:aca1f2f5443d | 95 | server.bind(SERVER_PORT); |
kaizen | 0:aca1f2f5443d | 96 | server.listen(); |
kaizen | 0:aca1f2f5443d | 97 | |
kaizen | 0:aca1f2f5443d | 98 | printf("\nWait for new connection...\r\n"); |
kaizen | 0:aca1f2f5443d | 99 | server.accept(client); |
kaizen | 0:aca1f2f5443d | 100 | client.set_blocking(false, 1500); |
kaizen | 0:aca1f2f5443d | 101 | } |
kaizen | 0:aca1f2f5443d | 102 | else if(speed_value <= 0.45) |
kaizen | 0:aca1f2f5443d | 103 | { |
kaizen | 0:aca1f2f5443d | 104 | printf("Speed decrease +\r\n"); |
kaizen | 0:aca1f2f5443d | 105 | speed_value += 0.05; |
kaizen | 0:aca1f2f5443d | 106 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 107 | } |
kaizen | 0:aca1f2f5443d | 108 | else if(speed_value >= 0.55) |
kaizen | 0:aca1f2f5443d | 109 | { |
kaizen | 0:aca1f2f5443d | 110 | printf("Speed decrease -\r\n"); |
kaizen | 0:aca1f2f5443d | 111 | speed_value -= 0.05; |
kaizen | 0:aca1f2f5443d | 112 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 113 | } |
kaizen | 0:aca1f2f5443d | 114 | } |
kaizen | 0:aca1f2f5443d | 115 | else |
kaizen | 0:aca1f2f5443d | 116 | { |
kaizen | 0:aca1f2f5443d | 117 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 118 | |
kaizen | 0:aca1f2f5443d | 119 | green_led = 0; red_led = 1; blue_led = 1; |
kaizen | 0:aca1f2f5443d | 120 | //recv_cnt = client.receive_all((char*)recv_control_msg, sizeof(control)-1); |
kaizen | 0:aca1f2f5443d | 121 | recv_cnt = client.receive_all((char*)recv_control_msg, sizeof(control)); |
kaizen | 0:aca1f2f5443d | 122 | |
kaizen | 0:aca1f2f5443d | 123 | j=0; |
kaizen | 0:aca1f2f5443d | 124 | parse(recv_control_msg, &j, control.start_msg); |
kaizen | 0:aca1f2f5443d | 125 | parse(recv_control_msg, &j, control.control_msg); |
kaizen | 0:aca1f2f5443d | 126 | parse(recv_control_msg, &j, control.speed_msg); |
kaizen | 0:aca1f2f5443d | 127 | parse(recv_control_msg, &j, control.end_msg); |
kaizen | 0:aca1f2f5443d | 128 | |
kaizen | 0:aca1f2f5443d | 129 | if(recv_cnt > 0) |
kaizen | 0:aca1f2f5443d | 130 | { |
kaizen | 0:aca1f2f5443d | 131 | if( (strncmp(control.start_msg,START_MSG,sizeof(control.start_msg)) != 0) || |
kaizen | 0:aca1f2f5443d | 132 | (strncmp(control.end_msg,END_MSG,sizeof(control.end_msg)) != 0) ) |
kaizen | 0:aca1f2f5443d | 133 | { |
kaizen | 0:aca1f2f5443d | 134 | printf("TEST Error\r\n"); |
kaizen | 0:aca1f2f5443d | 135 | sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_FAIL,END_MSG); |
kaizen | 0:aca1f2f5443d | 136 | client.send_all((char*)status_msg,strlen(status_msg)); |
kaizen | 0:aca1f2f5443d | 137 | continue; |
kaizen | 0:aca1f2f5443d | 138 | } |
kaizen | 0:aca1f2f5443d | 139 | |
kaizen | 0:aca1f2f5443d | 140 | if( strncmp(control.control_msg,CONTROL_UP,sizeof(control.control_msg)) == 0 ) |
kaizen | 0:aca1f2f5443d | 141 | { |
kaizen | 0:aca1f2f5443d | 142 | speed_value += 0.05; |
kaizen | 0:aca1f2f5443d | 143 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 144 | printf("TEST UP %f\r\n",speed_value); |
kaizen | 0:aca1f2f5443d | 145 | } |
kaizen | 0:aca1f2f5443d | 146 | else if( strncmp(control.control_msg,CONTROL_DOWN,sizeof(control.control_msg)) == 0) |
kaizen | 0:aca1f2f5443d | 147 | { |
kaizen | 0:aca1f2f5443d | 148 | speed_value -= 0.05; |
kaizen | 0:aca1f2f5443d | 149 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 150 | printf("TEST DOWN %f\r\n",speed_value); |
kaizen | 0:aca1f2f5443d | 151 | } |
kaizen | 0:aca1f2f5443d | 152 | else if( strncmp(control.control_msg,CONTROL_CRUISER,sizeof(control.control_msg)) == 0) |
kaizen | 0:aca1f2f5443d | 153 | { |
kaizen | 0:aca1f2f5443d | 154 | printf("TEST CRUISER\r\n"); |
kaizen | 0:aca1f2f5443d | 155 | speed_value = (float)(atoi(control.speed_msg)) / 100; |
kaizen | 0:aca1f2f5443d | 156 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 157 | } |
kaizen | 0:aca1f2f5443d | 158 | else if( strncmp(control.control_msg,CONTROL_STOP,sizeof(control.control_msg)) == 0) |
kaizen | 0:aca1f2f5443d | 159 | { |
kaizen | 0:aca1f2f5443d | 160 | speed_value = 0.5; |
kaizen | 0:aca1f2f5443d | 161 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 162 | printf("TEST STOP\r\n"); |
kaizen | 0:aca1f2f5443d | 163 | } |
kaizen | 0:aca1f2f5443d | 164 | else |
kaizen | 0:aca1f2f5443d | 165 | { |
kaizen | 0:aca1f2f5443d | 166 | printf("TEST Error 1-2\r\n"); |
kaizen | 0:aca1f2f5443d | 167 | sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_FAIL,END_MSG); |
kaizen | 0:aca1f2f5443d | 168 | client.send_all((char*)status_msg,strlen(status_msg)); |
kaizen | 0:aca1f2f5443d | 169 | continue; |
kaizen | 0:aca1f2f5443d | 170 | } |
kaizen | 0:aca1f2f5443d | 171 | |
kaizen | 0:aca1f2f5443d | 172 | sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_OK,END_MSG); |
kaizen | 0:aca1f2f5443d | 173 | client.send_all((char*)status_msg,strlen(status_msg)); |
kaizen | 0:aca1f2f5443d | 174 | } |
kaizen | 0:aca1f2f5443d | 175 | else |
kaizen | 0:aca1f2f5443d | 176 | { |
kaizen | 0:aca1f2f5443d | 177 | sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_OK,END_MSG); |
kaizen | 0:aca1f2f5443d | 178 | client.send_all((char*)status_msg,strlen(status_msg)); |
kaizen | 0:aca1f2f5443d | 179 | } |
kaizen | 0:aca1f2f5443d | 180 | } |
kaizen | 0:aca1f2f5443d | 181 | }// while |
kaizen | 0:aca1f2f5443d | 182 | } |
kaizen | 0:aca1f2f5443d | 183 | |
kaizen | 0:aca1f2f5443d | 184 | /* |
kaizen | 0:aca1f2f5443d | 185 | int main() { |
kaizen | 0:aca1f2f5443d | 186 | int recv_cnt; |
kaizen | 0:aca1f2f5443d | 187 | volatile float speed_value = 0.5; |
kaizen | 0:aca1f2f5443d | 188 | control_pkt skate_control_pkt; |
kaizen | 0:aca1f2f5443d | 189 | status_pkt skate_status_pkt; |
kaizen | 0:aca1f2f5443d | 190 | char recv_control_msg[100]; |
kaizen | 0:aca1f2f5443d | 191 | |
kaizen | 0:aca1f2f5443d | 192 | |
kaizen | 0:aca1f2f5443d | 193 | pc.baud(115200); |
kaizen | 0:aca1f2f5443d | 194 | green_led = 1; red_led = 1; blue_led = 1; |
kaizen | 0:aca1f2f5443d | 195 | |
kaizen | 0:aca1f2f5443d | 196 | wizfi250.init(); |
kaizen | 0:aca1f2f5443d | 197 | wizfi250.setAntMode(WizFi250::UFL); |
kaizen | 0:aca1f2f5443d | 198 | wizfi250.setAddress("192.168.100.2","255.255.255.0","192.168.100.1"); |
kaizen | 0:aca1f2f5443d | 199 | if ( wizfi250.connect(SECURE, SSID, PASS, WizFi250::WM_AP)) |
kaizen | 0:aca1f2f5443d | 200 | { |
kaizen | 0:aca1f2f5443d | 201 | red_led = 0; |
kaizen | 0:aca1f2f5443d | 202 | return -1; |
kaizen | 0:aca1f2f5443d | 203 | } |
kaizen | 0:aca1f2f5443d | 204 | printf("IP Address is %s\r\n", wizfi250.getIPAddress()); |
kaizen | 0:aca1f2f5443d | 205 | green_led = 0; red_led = 0; blue_led = 0; |
kaizen | 0:aca1f2f5443d | 206 | |
kaizen | 0:aca1f2f5443d | 207 | TCPSocketServer server; |
kaizen | 0:aca1f2f5443d | 208 | TCPSocketConnection client; |
kaizen | 0:aca1f2f5443d | 209 | |
kaizen | 0:aca1f2f5443d | 210 | while(true) |
kaizen | 0:aca1f2f5443d | 211 | { |
kaizen | 0:aca1f2f5443d | 212 | if( client.is_connected() == false ) |
kaizen | 0:aca1f2f5443d | 213 | { |
kaizen | 0:aca1f2f5443d | 214 | green_led = 1; red_led = 1; blue_led = 0; |
kaizen | 0:aca1f2f5443d | 215 | client.close(); |
kaizen | 0:aca1f2f5443d | 216 | server.close(); |
kaizen | 0:aca1f2f5443d | 217 | |
kaizen | 0:aca1f2f5443d | 218 | if(speed_value == 0.5) |
kaizen | 0:aca1f2f5443d | 219 | { |
kaizen | 0:aca1f2f5443d | 220 | server.bind(SERVER_PORT); |
kaizen | 0:aca1f2f5443d | 221 | server.listen(); |
kaizen | 0:aca1f2f5443d | 222 | |
kaizen | 0:aca1f2f5443d | 223 | printf("\nWait for new connection...\r\n"); |
kaizen | 0:aca1f2f5443d | 224 | server.accept(client); |
kaizen | 0:aca1f2f5443d | 225 | client.set_blocking(false, 1500); |
kaizen | 0:aca1f2f5443d | 226 | } |
kaizen | 0:aca1f2f5443d | 227 | else if(speed_value <= 0.45) |
kaizen | 0:aca1f2f5443d | 228 | { |
kaizen | 0:aca1f2f5443d | 229 | speed_value += 0.005; |
kaizen | 0:aca1f2f5443d | 230 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 231 | } |
kaizen | 0:aca1f2f5443d | 232 | else if(speed_value >= 0.55) |
kaizen | 0:aca1f2f5443d | 233 | { |
kaizen | 0:aca1f2f5443d | 234 | speed_value -= 0.005; |
kaizen | 0:aca1f2f5443d | 235 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 236 | } |
kaizen | 0:aca1f2f5443d | 237 | } |
kaizen | 0:aca1f2f5443d | 238 | else |
kaizen | 0:aca1f2f5443d | 239 | { |
kaizen | 0:aca1f2f5443d | 240 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 241 | |
kaizen | 0:aca1f2f5443d | 242 | green_led = 0; red_led = 1; blue_led = 1; |
kaizen | 0:aca1f2f5443d | 243 | //recv_cnt = client.receive_all((char*)&skate_control_pkt, sizeof(skate_control_pkt)); |
kaizen | 0:aca1f2f5443d | 244 | recv_cnt = client.receive_all((char*)recv_control_msg, sizeof(recv_control_msg)); |
kaizen | 0:aca1f2f5443d | 245 | |
kaizen | 0:aca1f2f5443d | 246 | if(recv_cnt > 0) |
kaizen | 0:aca1f2f5443d | 247 | { |
kaizen | 0:aca1f2f5443d | 248 | //printf("%x,%x,%x,%x\r\n",skate_control_pkt.start_signal,skate_control_pkt.control,skate_control_pkt.speed_value,skate_control_pkt.end_signal); |
kaizen | 0:aca1f2f5443d | 249 | |
kaizen | 0:aca1f2f5443d | 250 | printf("TEST %s\r\n", |
kaizen | 0:aca1f2f5443d | 251 | if( (skate_control_pkt.start_signal != START_SIGNAL) || (skate_control_pkt.end_signal != END_SIGNAL) ) |
kaizen | 0:aca1f2f5443d | 252 | { |
kaizen | 0:aca1f2f5443d | 253 | printf("TEST 100\r\n"); |
kaizen | 0:aca1f2f5443d | 254 | skate_status_pkt.start_signal = START_SIGNAL; |
kaizen | 0:aca1f2f5443d | 255 | skate_status_pkt.end_signal = END_SIGNAL; |
kaizen | 0:aca1f2f5443d | 256 | skate_status_pkt.speed_value = speed_value * 100; |
kaizen | 0:aca1f2f5443d | 257 | skate_status_pkt.status = STATUS_CMD_FAIL; |
kaizen | 0:aca1f2f5443d | 258 | client.send_all((char*)&skate_status_pkt, sizeof(skate_status_pkt)); |
kaizen | 0:aca1f2f5443d | 259 | continue; |
kaizen | 0:aca1f2f5443d | 260 | } |
kaizen | 0:aca1f2f5443d | 261 | |
kaizen | 0:aca1f2f5443d | 262 | if( skate_control_pkt.control == CONTROL_UP ) |
kaizen | 0:aca1f2f5443d | 263 | { |
kaizen | 0:aca1f2f5443d | 264 | speed_value += 0.005; |
kaizen | 0:aca1f2f5443d | 265 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 266 | printf("TEST UP %f\r\n",speed_value); |
kaizen | 0:aca1f2f5443d | 267 | } |
kaizen | 0:aca1f2f5443d | 268 | else if( skate_control_pkt.control == CONTROL_DOWN ) |
kaizen | 0:aca1f2f5443d | 269 | { |
kaizen | 0:aca1f2f5443d | 270 | speed_value -= 0.005; |
kaizen | 0:aca1f2f5443d | 271 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 272 | printf("TEST DOWN %f\r\n",speed_value); |
kaizen | 0:aca1f2f5443d | 273 | } |
kaizen | 0:aca1f2f5443d | 274 | else if( skate_control_pkt.control == CONTROL_CRUISER ) |
kaizen | 0:aca1f2f5443d | 275 | { |
kaizen | 0:aca1f2f5443d | 276 | speed_value = skate_control_pkt.speed_value / 100; |
kaizen | 0:aca1f2f5443d | 277 | motor_speed = speed_value; |
kaizen | 0:aca1f2f5443d | 278 | printf("TEST CRUISER\r\n"); |
kaizen | 0:aca1f2f5443d | 279 | } |
kaizen | 0:aca1f2f5443d | 280 | else |
kaizen | 0:aca1f2f5443d | 281 | { |
kaizen | 0:aca1f2f5443d | 282 | printf("TEST 101\r\n"); |
kaizen | 0:aca1f2f5443d | 283 | skate_status_pkt.start_signal = START_SIGNAL; |
kaizen | 0:aca1f2f5443d | 284 | skate_status_pkt.end_signal = END_SIGNAL; |
kaizen | 0:aca1f2f5443d | 285 | skate_status_pkt.status = STATUS_CMD_FAIL; |
kaizen | 0:aca1f2f5443d | 286 | skate_status_pkt.speed_value = speed_value * 100; |
kaizen | 0:aca1f2f5443d | 287 | client.send_all((char*)&skate_status_pkt, sizeof(skate_status_pkt)); |
kaizen | 0:aca1f2f5443d | 288 | continue; |
kaizen | 0:aca1f2f5443d | 289 | } |
kaizen | 0:aca1f2f5443d | 290 | printf("TEST 103\r\n"); |
kaizen | 0:aca1f2f5443d | 291 | |
kaizen | 0:aca1f2f5443d | 292 | skate_status_pkt.start_signal = START_SIGNAL; |
kaizen | 0:aca1f2f5443d | 293 | skate_status_pkt.end_signal = END_SIGNAL; |
kaizen | 0:aca1f2f5443d | 294 | skate_status_pkt.status = STATUS_CMD_OK; |
kaizen | 0:aca1f2f5443d | 295 | skate_status_pkt.speed_value = speed_value * 100; |
kaizen | 0:aca1f2f5443d | 296 | client.send_all((char*)&skate_status_pkt, sizeof(skate_status_pkt)); |
kaizen | 0:aca1f2f5443d | 297 | } |
kaizen | 0:aca1f2f5443d | 298 | } |
kaizen | 0:aca1f2f5443d | 299 | }// while |
kaizen | 0:aca1f2f5443d | 300 | } |
kaizen | 0:aca1f2f5443d | 301 | */ |
kaizen | 0:aca1f2f5443d | 302 | |
kaizen | 0:aca1f2f5443d | 303 | void parse(char buffer[], int *j, char *string) { |
kaizen | 0:aca1f2f5443d | 304 | //extracts next location string data item from buffer |
kaizen | 0:aca1f2f5443d | 305 | int i=0; |
kaizen | 0:aca1f2f5443d | 306 | for (i=0; i<=strlen(buffer); i++) { //TOTAL SIZE OF RETURNED DATA |
kaizen | 0:aca1f2f5443d | 307 | if ((buffer[*j+i] == ',')||(buffer[*j+i] == '\0' )) { //IF comma or end of string |
kaizen | 0:aca1f2f5443d | 308 | //comma is the string field delimiter |
kaizen | 0:aca1f2f5443d | 309 | string[i]=0; //SETS END OF SRTRING TO 0 |
kaizen | 0:aca1f2f5443d | 310 | *j=*j+i+1; //UPDATES to 1 after comma seperated value |
kaizen | 0:aca1f2f5443d | 311 | break; |
kaizen | 0:aca1f2f5443d | 312 | } else string[i]=buffer[*j+i]; //Keep adding to the string |
kaizen | 0:aca1f2f5443d | 313 | } |
kaizen | 0:aca1f2f5443d | 314 | } |