Bjorn Van Bockel / Mbed 2 deprecated EduRobot

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?

UserRevisionLine numberNew 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 }