code voor edu_robot
Dependencies: LM75B SRF02 mbed
main.cpp@0:4b909635e2d2, 2014-02-26 (annotated)
- Committer:
- BjornVB
- Date:
- Wed Feb 26 08:31:04 2014 +0000
- Revision:
- 0:4b909635e2d2
Versie met mogelijke aanpassingen naar firmware 4.00 wifly
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BjornVB | 0:4b909635e2d2 | 1 | #include "mbed.h" |
BjornVB | 0:4b909635e2d2 | 2 | #include "LM75B.h" |
BjornVB | 0:4b909635e2d2 | 3 | #include "SRF02.h" |
BjornVB | 0:4b909635e2d2 | 4 | #include <string> |
BjornVB | 0:4b909635e2d2 | 5 | #include <stdio.h> |
BjornVB | 0:4b909635e2d2 | 6 | #include <stdlib.h> |
BjornVB | 0:4b909635e2d2 | 7 | |
BjornVB | 0:4b909635e2d2 | 8 | /***********Components****************/ |
BjornVB | 0:4b909635e2d2 | 9 | Serial pc(USBTX, USBRX); //PC debug |
BjornVB | 0:4b909635e2d2 | 10 | Serial wifi(p9,p10); //poort voor de wifly module |
BjornVB | 0:4b909635e2d2 | 11 | LM75B sensor(p28,p27); //Temperatuur sensor |
BjornVB | 0:4b909635e2d2 | 12 | SRF02 srf02(p28, p27, 0xE0, 0x51); //ultrasoon range sensor |
BjornVB | 0:4b909635e2d2 | 13 | PwmOut R_V(p21); //PWM |
BjornVB | 0:4b909635e2d2 | 14 | PwmOut R_A(p22); |
BjornVB | 0:4b909635e2d2 | 15 | PwmOut L_V(p23); |
BjornVB | 0:4b909635e2d2 | 16 | PwmOut L_A(p24); |
BjornVB | 0:4b909635e2d2 | 17 | |
BjornVB | 0:4b909635e2d2 | 18 | /***********Variables****************/ |
BjornVB | 0:4b909635e2d2 | 19 | string str = ""; |
BjornVB | 0:4b909635e2d2 | 20 | char temp[8]; //char array voor temperatuur data |
BjornVB | 0:4b909635e2d2 | 21 | char afstand[8]; //char array voor afstand data |
BjornVB | 0:4b909635e2d2 | 22 | char c; //c slaagt inkomenbde karakters op |
BjornVB | 0:4b909635e2d2 | 23 | |
BjornVB | 0:4b909635e2d2 | 24 | /***********Functions****************/ |
BjornVB | 0:4b909635e2d2 | 25 | void send(const char * str); |
BjornVB | 0:4b909635e2d2 | 26 | void write_wifi(const char * msg); |
BjornVB | 0:4b909635e2d2 | 27 | void stuurmotors(float Rechts_Vooruit, float Rechts_Achteruit, float Links_Vooruit, float Links_Achteruit); |
BjornVB | 0:4b909635e2d2 | 28 | void richting(char c,float value); |
BjornVB | 0:4b909635e2d2 | 29 | void wifi_config(); |
BjornVB | 0:4b909635e2d2 | 30 | |
BjornVB | 0:4b909635e2d2 | 31 | /***********main*********************/ |
BjornVB | 0:4b909635e2d2 | 32 | int main() |
BjornVB | 0:4b909635e2d2 | 33 | { |
BjornVB | 0:4b909635e2d2 | 34 | string pwm = ""; //deel van string met pwm waarde |
BjornVB | 0:4b909635e2d2 | 35 | int value =0; //variabele voor de omzetting van string van pwm |
BjornVB | 0:4b909635e2d2 | 36 | pc.printf("wifly TCP Read!\r\n"); //print test wifly!! |
BjornVB | 0:4b909635e2d2 | 37 | |
BjornVB | 0:4b909635e2d2 | 38 | wifi_config(); //functie om adhoc op te zetten |
BjornVB | 0:4b909635e2d2 | 39 | |
BjornVB | 0:4b909635e2d2 | 40 | if (sensor.open()) { |
BjornVB | 0:4b909635e2d2 | 41 | pc.printf("Devices detected!\n\r"); |
BjornVB | 0:4b909635e2d2 | 42 | pc.printf("Temp: %.3f C\n\r", (float)sensor); |
BjornVB | 0:4b909635e2d2 | 43 | pc.printf("Range: %.2f cm\n\r",srf02.read()); |
BjornVB | 0:4b909635e2d2 | 44 | } |
BjornVB | 0:4b909635e2d2 | 45 | |
BjornVB | 0:4b909635e2d2 | 46 | while(1) { |
BjornVB | 0:4b909635e2d2 | 47 | wait(0.01); |
BjornVB | 0:4b909635e2d2 | 48 | sprintf(temp, "%5.2f",(float)sensor); |
BjornVB | 0:4b909635e2d2 | 49 | sprintf(afstand, "%5.2f",srf02.read()); |
BjornVB | 0:4b909635e2d2 | 50 | while(wifi.readable()) { |
BjornVB | 0:4b909635e2d2 | 51 | c = wifi.getc(); //inkomend karakter is opgeslagen in c |
BjornVB | 0:4b909635e2d2 | 52 | if(c == '*') { //wanneer inkomend karakter gelijk is aan '*' |
BjornVB | 0:4b909635e2d2 | 53 | pwm = str.substr(3,2); //laatste deel van string opslaan in string pwm |
BjornVB | 0:4b909635e2d2 | 54 | value = (double)atof(pwm.c_str()); //string van pwm omzetten in double |
BjornVB | 0:4b909635e2d2 | 55 | richting(str[0],value); //richting bepalen + motoren juist aansturen |
BjornVB | 0:4b909635e2d2 | 56 | str = ""; //string terug leegmaken |
BjornVB | 0:4b909635e2d2 | 57 | } else { |
BjornVB | 0:4b909635e2d2 | 58 | str = str + c; //als karakter nog niet gelijk is aan '*' string vullen |
BjornVB | 0:4b909635e2d2 | 59 | } |
BjornVB | 0:4b909635e2d2 | 60 | } |
BjornVB | 0:4b909635e2d2 | 61 | } |
BjornVB | 0:4b909635e2d2 | 62 | } |
BjornVB | 0:4b909635e2d2 | 63 | |
BjornVB | 0:4b909635e2d2 | 64 | void send(const char * str) //functie om commando te schrijven naar wifly |
BjornVB | 0:4b909635e2d2 | 65 | { |
BjornVB | 0:4b909635e2d2 | 66 | int len = strlen(str); |
BjornVB | 0:4b909635e2d2 | 67 | for(int i = 0; i < len; i++) { |
BjornVB | 0:4b909635e2d2 | 68 | wifi.putc(str[i]); //schrijft commando karakter per karakter in de wifly |
BjornVB | 0:4b909635e2d2 | 69 | } |
BjornVB | 0:4b909635e2d2 | 70 | } |
BjornVB | 0:4b909635e2d2 | 71 | |
BjornVB | 0:4b909635e2d2 | 72 | void write_wifi(const char * msg) |
BjornVB | 0:4b909635e2d2 | 73 | { |
BjornVB | 0:4b909635e2d2 | 74 | while(wifi.writeable()) { |
BjornVB | 0:4b909635e2d2 | 75 | send(msg); |
BjornVB | 0:4b909635e2d2 | 76 | } |
BjornVB | 0:4b909635e2d2 | 77 | } |
BjornVB | 0:4b909635e2d2 | 78 | |
BjornVB | 0:4b909635e2d2 | 79 | void stuurmotors(float Rechts_Vooruit, float Rechts_Achteruit, float Links_Vooruit, float Links_Achteruit) |
BjornVB | 0:4b909635e2d2 | 80 | { |
BjornVB | 0:4b909635e2d2 | 81 | R_V = Rechts_Vooruit; |
BjornVB | 0:4b909635e2d2 | 82 | R_A = Rechts_Achteruit; |
BjornVB | 0:4b909635e2d2 | 83 | L_V = Links_Vooruit; |
BjornVB | 0:4b909635e2d2 | 84 | L_A = Links_Achteruit; |
BjornVB | 0:4b909635e2d2 | 85 | |
BjornVB | 0:4b909635e2d2 | 86 | } |
BjornVB | 0:4b909635e2d2 | 87 | |
BjornVB | 0:4b909635e2d2 | 88 | void richting(char c,float value) //functie bepaalt richting en pwm waarde |
BjornVB | 0:4b909635e2d2 | 89 | { |
BjornVB | 0:4b909635e2d2 | 90 | float waarde; |
BjornVB | 0:4b909635e2d2 | 91 | waarde = value/100; //value is waarde achter de komma, value/100 is komma getal geschikt voor pwm |
BjornVB | 0:4b909635e2d2 | 92 | |
BjornVB | 0:4b909635e2d2 | 93 | switch (c) { |
BjornVB | 0:4b909635e2d2 | 94 | case '0': |
BjornVB | 0:4b909635e2d2 | 95 | stuurmotors(0, 0, 0, 0); |
BjornVB | 0:4b909635e2d2 | 96 | break; |
BjornVB | 0:4b909635e2d2 | 97 | case '1': |
BjornVB | 0:4b909635e2d2 | 98 | stuurmotors(waarde, 0, waarde, 0); |
BjornVB | 0:4b909635e2d2 | 99 | break; |
BjornVB | 0:4b909635e2d2 | 100 | case '2': |
BjornVB | 0:4b909635e2d2 | 101 | stuurmotors(0, waarde, 0, waarde); |
BjornVB | 0:4b909635e2d2 | 102 | break; |
BjornVB | 0:4b909635e2d2 | 103 | case '3': |
BjornVB | 0:4b909635e2d2 | 104 | stuurmotors(0, waarde, waarde, 0); |
BjornVB | 0:4b909635e2d2 | 105 | break; |
BjornVB | 0:4b909635e2d2 | 106 | case '4': |
BjornVB | 0:4b909635e2d2 | 107 | stuurmotors(waarde, 0, 0, waarde); |
BjornVB | 0:4b909635e2d2 | 108 | break; |
BjornVB | 0:4b909635e2d2 | 109 | case '5': |
BjornVB | 0:4b909635e2d2 | 110 | write_wifi(temp); |
BjornVB | 0:4b909635e2d2 | 111 | break; |
BjornVB | 0:4b909635e2d2 | 112 | case '6': |
BjornVB | 0:4b909635e2d2 | 113 | write_wifi(afstand); |
BjornVB | 0:4b909635e2d2 | 114 | break; |
BjornVB | 0:4b909635e2d2 | 115 | default: |
BjornVB | 0:4b909635e2d2 | 116 | stuurmotors(0, 0, 0, 0); |
BjornVB | 0:4b909635e2d2 | 117 | write_wifi("ERROR!"); |
BjornVB | 0:4b909635e2d2 | 118 | } |
BjornVB | 0:4b909635e2d2 | 119 | } |
BjornVB | 0:4b909635e2d2 | 120 | |
BjornVB | 0:4b909635e2d2 | 121 | void wifi_config() //wifi_config schrijft commando's in wifly om adhoc op te zetten |
BjornVB | 0:4b909635e2d2 | 122 | { |
BjornVB | 0:4b909635e2d2 | 123 | send("$$$"); //commando om wifly in command mode te zetten |
BjornVB | 0:4b909635e2d2 | 124 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 125 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 126 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 127 | |
BjornVB | 0:4b909635e2d2 | 128 | send("set w j 4\r"); //set wlan join 4 |
BjornVB | 0:4b909635e2d2 | 129 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 130 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 131 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 132 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 133 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 134 | |
BjornVB | 0:4b909635e2d2 | 135 | send("set w s Edu_Robot\r"); //set wlan ssid EDU_robot |
BjornVB | 0:4b909635e2d2 | 136 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 137 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 138 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 139 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 140 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 141 | |
BjornVB | 0:4b909635e2d2 | 142 | send("set w c 2\r"); //set wlan channel 0 |
BjornVB | 0:4b909635e2d2 | 143 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 144 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 145 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 146 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 147 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 148 | |
BjornVB | 0:4b909635e2d2 | 149 | send("set i a 169.254.1.1\r"); //set ip adress 169.254.1.1 |
BjornVB | 0:4b909635e2d2 | 150 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 151 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 152 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 153 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 154 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 155 | |
BjornVB | 0:4b909635e2d2 | 156 | send("set i n 255.255.0.0\r"); //set ip netmask 255.255.0.0 |
BjornVB | 0:4b909635e2d2 | 157 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 158 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 159 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 160 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 161 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 162 | |
BjornVB | 0:4b909635e2d2 | 163 | send("set i d 0\r"); //set ip dhcp 0 |
BjornVB | 0:4b909635e2d2 | 164 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 165 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 166 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 167 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 168 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 169 | |
BjornVB | 0:4b909635e2d2 | 170 | send("set c c 0\r"); //set command close * |
BjornVB | 0:4b909635e2d2 | 171 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 172 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 173 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 174 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 175 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 176 | |
BjornVB | 0:4b909635e2d2 | 177 | send("set c o 0\r"); //set command open 0 |
BjornVB | 0:4b909635e2d2 | 178 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 179 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 180 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 181 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 182 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 183 | |
BjornVB | 0:4b909635e2d2 | 184 | send("set c r 0\r"); //set command remote 0 |
BjornVB | 0:4b909635e2d2 | 185 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 186 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 187 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 188 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 189 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 190 | |
BjornVB | 0:4b909635e2d2 | 191 | send("save\r"); //save settings |
BjornVB | 0:4b909635e2d2 | 192 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 193 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 194 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 195 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 196 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 197 | |
BjornVB | 0:4b909635e2d2 | 198 | send("reboot\r"); //reboot module |
BjornVB | 0:4b909635e2d2 | 199 | pc.putc(wifi.getc()); //print acknowledge |
BjornVB | 0:4b909635e2d2 | 200 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 201 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 202 | while(wifi.readable()) //print ack |
BjornVB | 0:4b909635e2d2 | 203 | pc.putc(wifi.getc()); |
BjornVB | 0:4b909635e2d2 | 204 | wait(1); |
BjornVB | 0:4b909635e2d2 | 205 | } |