IOTON Technology / ton-bot_seguidor_linha

Programa de exemplo do Robô TON-BOT no modo seguidor de linha.

Dependencies:   IOTON-API QEI USBDevice mbed-ton ton-bot

Fork of ton-bot_teste by IOTON Technology

main.cpp@0:52cc151dc505, 2017-06-29 (annotated)

Committer:
krebyy
Date:
Thu Jun 29 20:37:24 2017 +0000
Revision:
0:52cc151dc505
Child:
1:584ac95c4061
First commit - ioton.cc/demo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krebyy 0:52cc151dc505 1 // Includes --------------------------------------------------------------------
krebyy 0:52cc151dc505 2 #include "Ioton.h"
krebyy 0:52cc151dc505 3
krebyy 0:52cc151dc505 4 // Defines ---------------------------------------------------------------------
krebyy 0:52cc151dc505 5 #define MY_SSID "mySSID" // Enter router ssid inside the quotes
krebyy 0:52cc151dc505 6 #define MY_PASS "myPASS" // Enter router password inside the quotes
krebyy 0:52cc151dc505 7 #define MY_API_KEY "myAPIKEY" // Enter ThingSpeak API KEY of channel
krebyy 0:52cc151dc505 8
krebyy 0:52cc151dc505 9
krebyy 0:52cc151dc505 10 // Private variables -----------------------------------------------------------
krebyy 0:52cc151dc505 11 PwmOut pwm(PIN13); // PWM output
krebyy 0:52cc151dc505 12 DigitalOut out(PIN14); // Digital output
krebyy 0:52cc151dc505 13 AnalogIn ain(PIN15); // Analog input
krebyy 0:52cc151dc505 14 DigitalIn in(PIN16); // Digital input
krebyy 0:52cc151dc505 15
krebyy 0:52cc151dc505 16 InterruptIn button(SW_USER); // USER button - interrupt mode
krebyy 0:52cc151dc505 17 int userCount = 0; // USER button counter
krebyy 0:52cc151dc505 18
krebyy 0:52cc151dc505 19 char bufferRxBluetooth[20]; // Buffer of Bluetooth commands
krebyy 0:52cc151dc505 20 volatile uint8_t bufferCountRxBluetooth = 0; // Index counter - Blutooth buffer
krebyy 0:52cc151dc505 21 volatile bool flagCmdBluetooth = false; // Indicates new Bluetooth command received
krebyy 0:52cc151dc505 22
krebyy 0:52cc151dc505 23 char* replyHttp; // Buffer of reply http get method
krebyy 0:52cc151dc505 24 uint8_t countThingSpeak = 0; // Auxiliar counter - ThingSpeak
krebyy 0:52cc151dc505 25
krebyy 0:52cc151dc505 26 Ticker imuTick; // IMU Tick Timer
krebyy 0:52cc151dc505 27
krebyy 0:52cc151dc505 28
krebyy 0:52cc151dc505 29 // *****************************************************************************
krebyy 0:52cc151dc505 30 // Private functions ***********************************************************
krebyy 0:52cc151dc505 31 // *****************************************************************************
krebyy 0:52cc151dc505 32 // Runs the AHRS algorithm - Rate: 0.1 s - 10 Hz -------------------------------
krebyy 0:52cc151dc505 33 // AHRS provide: roll, pitch and yaw
krebyy 0:52cc151dc505 34 void imuCallback(void)
krebyy 0:52cc151dc505 35 {
krebyy 0:52cc151dc505 36 // imu.runAHRS(0.1); // ### FIX: BUG TICKER
krebyy 0:52cc151dc505 37 } // end of imuCallback
krebyy 0:52cc151dc505 38
krebyy 0:52cc151dc505 39
krebyy 0:52cc151dc505 40 // Called everytime a new character goes into the RX buffer --------------------
krebyy 0:52cc151dc505 41 void bluetoothRxCallback(void)
krebyy 0:52cc151dc505 42 {
krebyy 0:52cc151dc505 43 bufferRxBluetooth[bufferCountRxBluetooth] = bluetooth.getc();
krebyy 0:52cc151dc505 44 bufferCountRxBluetooth++;
krebyy 0:52cc151dc505 45
krebyy 0:52cc151dc505 46 if (bluetooth.readable() == 0) flagCmdBluetooth = true;
krebyy 0:52cc151dc505 47 } // end of bluetoothRxCallback
krebyy 0:52cc151dc505 48
krebyy 0:52cc151dc505 49
krebyy 0:52cc151dc505 50 // USER button interrupt callback ----------------------------------------------
krebyy 0:52cc151dc505 51 void userCallback(void)
krebyy 0:52cc151dc505 52 {
krebyy 0:52cc151dc505 53 userCount++;
krebyy 0:52cc151dc505 54 } // end of userCallback
krebyy 0:52cc151dc505 55
krebyy 0:52cc151dc505 56
krebyy 0:52cc151dc505 57 // Parse the Bluetooth commands ------------------------------------------------
krebyy 0:52cc151dc505 58 // COMMAND | DESCRIPTION
krebyy 0:52cc151dc505 59 // -----------------------------------------------------------------------------
krebyy 0:52cc151dc505 60 // #PWM:float | Update pwm output. Ex: #PWM:0.5
krebyy 0:52cc151dc505 61 // #OUT:int | Update digital output. Ex: #OUT:1
krebyy 0:52cc151dc505 62 // #RGB:rgbcode | Update LED RGB color. Ex: #RGB:00AFEF
krebyy 0:52cc151dc505 63 // #AIN? | Return analog input
krebyy 0:52cc151dc505 64 // #DIN? | Return digital input
krebyy 0:52cc151dc505 65 // #BAT? | Return battery voltage
krebyy 0:52cc151dc505 66 // #IMU? | Return imu data (roll, pitch and yaw)
krebyy 0:52cc151dc505 67 // #ALL? | Return all values
krebyy 0:52cc151dc505 68 void parseCmdBluetooth(char* str)
krebyy 0:52cc151dc505 69 {
krebyy 0:52cc151dc505 70 if (strncmp("#PWM:", str, 5) == 0)
krebyy 0:52cc151dc505 71 {
krebyy 0:52cc151dc505 72 pwm = strtof(str + 5, NULL);
krebyy 0:52cc151dc505 73 }
krebyy 0:52cc151dc505 74 else if (strncmp("#OUT:", str, 5) == 0)
krebyy 0:52cc151dc505 75 {
krebyy 0:52cc151dc505 76 out = atoi(str + 5);
krebyy 0:52cc151dc505 77 }
krebyy 0:52cc151dc505 78 else if (strncmp("#RGB:", str, 5) == 0)
krebyy 0:52cc151dc505 79 {
krebyy 0:52cc151dc505 80 ton.setLED(str + 5);
krebyy 0:52cc151dc505 81 }
krebyy 0:52cc151dc505 82 else if (strncmp("#AIN?", str, 5) == 0)
krebyy 0:52cc151dc505 83 {
krebyy 0:52cc151dc505 84 bluetooth.printf("#AIN:%0.2f\r\n", ain.read());
krebyy 0:52cc151dc505 85 }
krebyy 0:52cc151dc505 86 else if (strncmp("#DIN?", str, 5) == 0)
krebyy 0:52cc151dc505 87 {
krebyy 0:52cc151dc505 88 bluetooth.printf("#DIN:%d\r\n", in.read());
krebyy 0:52cc151dc505 89 }
krebyy 0:52cc151dc505 90 else if (strncmp("#BAT?", str, 5) == 0)
krebyy 0:52cc151dc505 91 {
krebyy 0:52cc151dc505 92 bluetooth.printf("#BAT:%0.2fV\r\n", ton.getBattery());
krebyy 0:52cc151dc505 93 }
krebyy 0:52cc151dc505 94 else if (strncmp("#IMU?", str, 5) == 0)
krebyy 0:52cc151dc505 95 {
krebyy 0:52cc151dc505 96 bluetooth.printf("pitch: %0.2f\r\nroll: %0.2f\r\nyaw: %0.2f\r\n",
krebyy 0:52cc151dc505 97 imu.getPitch(), imu.getRoll(), imu.getYaw());
krebyy 0:52cc151dc505 98 }
krebyy 0:52cc151dc505 99 else if (strncmp("#ALL?", str, 5) == 0)
krebyy 0:52cc151dc505 100 {
krebyy 0:52cc151dc505 101 bluetooth.printf("#AIN:%0.2f\r\n#DIN:%d\r\n#BAT:%0.2fV\r\n",
krebyy 0:52cc151dc505 102 ain.read(), in.read(), ton.getBattery());
krebyy 0:52cc151dc505 103
krebyy 0:52cc151dc505 104 bluetooth.printf("pitch: %0.2f\r\nroll: %0.2f\r\nyaw: %0.2f\r\n",
krebyy 0:52cc151dc505 105 imu.getPitch(), imu.getRoll(), imu.getYaw());
krebyy 0:52cc151dc505 106 }
krebyy 0:52cc151dc505 107 else
krebyy 0:52cc151dc505 108 {
krebyy 0:52cc151dc505 109 bluetooth.printf("Invalid command!\r\n");
krebyy 0:52cc151dc505 110 }
krebyy 0:52cc151dc505 111 } // end of parseCmdBluetooth function
krebyy 0:52cc151dc505 112
krebyy 0:52cc151dc505 113
krebyy 0:52cc151dc505 114 // Data format: #rgbcode,float,int | Ex: #00AFEF,0.5,1 ------------------------
krebyy 0:52cc151dc505 115 void parseCmdWifi(char* str)
krebyy 0:52cc151dc505 116 {
krebyy 0:52cc151dc505 117 char* tmp = strstr(str, "#"); // Search the first character of the command
krebyy 0:52cc151dc505 118
krebyy 0:52cc151dc505 119 if (tmp != NULL)
krebyy 0:52cc151dc505 120 {
krebyy 0:52cc151dc505 121 tmp = strtok(tmp, ",\n");
krebyy 0:52cc151dc505 122 ton.setLED(tmp + 1); // Remove the '#' and update LED RGB color
krebyy 0:52cc151dc505 123
krebyy 0:52cc151dc505 124 tmp = strtok(NULL,",\n");
krebyy 0:52cc151dc505 125 pwm = strtof(tmp, NULL); // Convert to float and update pwm output
krebyy 0:52cc151dc505 126
krebyy 0:52cc151dc505 127 tmp = strtok(NULL,",\n");
krebyy 0:52cc151dc505 128 out = atoi(tmp); // Convert to int and update digital output
krebyy 0:52cc151dc505 129 }
krebyy 0:52cc151dc505 130 } // end of parseCmdWifi function
krebyy 0:52cc151dc505 131
krebyy 0:52cc151dc505 132
krebyy 0:52cc151dc505 133 // *****************************************************************************
krebyy 0:52cc151dc505 134 // MAIN PROGRAM ****************************************************************
krebyy 0:52cc151dc505 135 // *****************************************************************************
krebyy 0:52cc151dc505 136 int main(void)
krebyy 0:52cc151dc505 137 {
krebyy 0:52cc151dc505 138 // Initializations
krebyy 0:52cc151dc505 139 ton.setLED(RED);
krebyy 0:52cc151dc505 140 ton.enableBluetooth();
krebyy 0:52cc151dc505 141 ton.enableWifi();
krebyy 0:52cc151dc505 142 ton.enableIMU(); // Leave standing the TON board at initialization (~5s)
krebyy 0:52cc151dc505 143 ton.setLED(BLUE);
krebyy 0:52cc151dc505 144
krebyy 0:52cc151dc505 145 // Configure Tick Timer for IMU reads - interval (0.1 second)
krebyy 0:52cc151dc505 146 imuTick.attach(&imuCallback, 0.1);
krebyy 0:52cc151dc505 147
krebyy 0:52cc151dc505 148 // Configure RX interrupt of Bluetooth
krebyy 0:52cc151dc505 149 bluetooth.attach(&bluetoothRxCallback, Serial::RxIrq);
krebyy 0:52cc151dc505 150
krebyy 0:52cc151dc505 151 // Try to connect to the access point
krebyy 0:52cc151dc505 152 if (wifi.connect(MY_SSID, MY_PASS) == true) ton.setLED(GREEN);
krebyy 0:52cc151dc505 153 else ton.setLED(YELLOW);
krebyy 0:52cc151dc505 154
krebyy 0:52cc151dc505 155 // Configure USER button interrupt
krebyy 0:52cc151dc505 156 button.rise(&userCallback);
krebyy 0:52cc151dc505 157
krebyy 0:52cc151dc505 158 // The main LOOP -----------------------------------------------------------
krebyy 0:52cc151dc505 159 while (1)
krebyy 0:52cc151dc505 160 {
krebyy 0:52cc151dc505 161 // Prints messages to the USB ------------------------------------------
krebyy 0:52cc151dc505 162 usb.printf("pitch: %0.3f\r\n", imu.getPitch());
krebyy 0:52cc151dc505 163 usb.printf("roll: %0.3f\r\n", imu.getRoll());
krebyy 0:52cc151dc505 164 usb.printf("yaw: %0.3f\r\n", imu.getYaw());
krebyy 0:52cc151dc505 165 usb.printf("---\r\n");
krebyy 0:52cc151dc505 166 usb.printf("battery: %0.3fV\r\n", ton.getBattery());
krebyy 0:52cc151dc505 167 usb.printf("count: %d\r\n", userCount);
krebyy 0:52cc151dc505 168 usb.printf("\r\n");
krebyy 0:52cc151dc505 169
krebyy 0:52cc151dc505 170 // Checks new Blutooth commands ----------------------------------------
krebyy 0:52cc151dc505 171 if (flagCmdBluetooth == true)
krebyy 0:52cc151dc505 172 {
krebyy 0:52cc151dc505 173 // Parse the Bluetooth commands
krebyy 0:52cc151dc505 174 parseCmdBluetooth(bufferRxBluetooth);
krebyy 0:52cc151dc505 175
krebyy 0:52cc151dc505 176 bufferCountRxBluetooth = 0;
krebyy 0:52cc151dc505 177 memset(bufferRxBluetooth, 0, sizeof(bufferRxBluetooth));
krebyy 0:52cc151dc505 178 flagCmdBluetooth = false;
krebyy 0:52cc151dc505 179 } // end of check new Bluetooth commands
krebyy 0:52cc151dc505 180
krebyy 0:52cc151dc505 181 // Checks if wifi is connected and run wifi tasks ----------------------
krebyy 0:52cc151dc505 182 if (wifi.isConnected() == true)
krebyy 0:52cc151dc505 183 {
krebyy 0:52cc151dc505 184 // Parse the http get method
krebyy 0:52cc151dc505 185 replyHttp = wifi.httpGet("ioton.cc", "/ton-demo.txt");
krebyy 0:52cc151dc505 186 parseCmdWifi(replyHttp);
krebyy 0:52cc151dc505 187
krebyy 0:52cc151dc505 188 // Send data via ThingSpeak (does not support high rates)
krebyy 0:52cc151dc505 189 if (++countThingSpeak == 5)
krebyy 0:52cc151dc505 190 {
krebyy 0:52cc151dc505 191 char thingspeak[50];
krebyy 0:52cc151dc505 192
krebyy 0:52cc151dc505 193 sprintf(thingspeak, "field1=%0.3f&field2=%d&field3=%0.3f",
krebyy 0:52cc151dc505 194 ain.read(), in.read(), ton.getBattery());
krebyy 0:52cc151dc505 195
krebyy 0:52cc151dc505 196 usb.printf("Send to ThingSpeak: %s\r\n", thingspeak);
krebyy 0:52cc151dc505 197 wifi.sendThingSpeak(thingspeak, MY_API_KEY);
krebyy 0:52cc151dc505 198
krebyy 0:52cc151dc505 199 countThingSpeak = 0;
krebyy 0:52cc151dc505 200 } // end of send to ThingSpeak
krebyy 0:52cc151dc505 201 } // end of wifi tasks
krebyy 0:52cc151dc505 202
krebyy 0:52cc151dc505 203 wait(1);
krebyy 0:52cc151dc505 204 } // end of main LOOP
krebyy 0:52cc151dc505 205 } // end of main function