![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
code voor edu_robot
Dependencies: LM75B SRF02 mbed
Diff: main.cpp
- Revision:
- 0:4b909635e2d2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Feb 26 08:31:04 2014 +0000 @@ -0,0 +1,205 @@ +#include "mbed.h" +#include "LM75B.h" +#include "SRF02.h" +#include <string> +#include <stdio.h> +#include <stdlib.h> + +/***********Components****************/ +Serial pc(USBTX, USBRX); //PC debug +Serial wifi(p9,p10); //poort voor de wifly module +LM75B sensor(p28,p27); //Temperatuur sensor +SRF02 srf02(p28, p27, 0xE0, 0x51); //ultrasoon range sensor +PwmOut R_V(p21); //PWM +PwmOut R_A(p22); +PwmOut L_V(p23); +PwmOut L_A(p24); + +/***********Variables****************/ +string str = ""; +char temp[8]; //char array voor temperatuur data +char afstand[8]; //char array voor afstand data +char c; //c slaagt inkomenbde karakters op + +/***********Functions****************/ +void send(const char * str); +void write_wifi(const char * msg); +void stuurmotors(float Rechts_Vooruit, float Rechts_Achteruit, float Links_Vooruit, float Links_Achteruit); +void richting(char c,float value); +void wifi_config(); + +/***********main*********************/ +int main() +{ + string pwm = ""; //deel van string met pwm waarde + int value =0; //variabele voor de omzetting van string van pwm + pc.printf("wifly TCP Read!\r\n"); //print test wifly!! + + wifi_config(); //functie om adhoc op te zetten + + if (sensor.open()) { + pc.printf("Devices detected!\n\r"); + pc.printf("Temp: %.3f C\n\r", (float)sensor); + pc.printf("Range: %.2f cm\n\r",srf02.read()); + } + + while(1) { + wait(0.01); + sprintf(temp, "%5.2f",(float)sensor); + sprintf(afstand, "%5.2f",srf02.read()); + while(wifi.readable()) { + c = wifi.getc(); //inkomend karakter is opgeslagen in c + if(c == '*') { //wanneer inkomend karakter gelijk is aan '*' + pwm = str.substr(3,2); //laatste deel van string opslaan in string pwm + value = (double)atof(pwm.c_str()); //string van pwm omzetten in double + richting(str[0],value); //richting bepalen + motoren juist aansturen + str = ""; //string terug leegmaken + } else { + str = str + c; //als karakter nog niet gelijk is aan '*' string vullen + } + } + } +} + +void send(const char * str) //functie om commando te schrijven naar wifly +{ + int len = strlen(str); + for(int i = 0; i < len; i++) { + wifi.putc(str[i]); //schrijft commando karakter per karakter in de wifly + } +} + +void write_wifi(const char * msg) +{ + while(wifi.writeable()) { + send(msg); + } +} + +void stuurmotors(float Rechts_Vooruit, float Rechts_Achteruit, float Links_Vooruit, float Links_Achteruit) +{ + R_V = Rechts_Vooruit; + R_A = Rechts_Achteruit; + L_V = Links_Vooruit; + L_A = Links_Achteruit; + +} + +void richting(char c,float value) //functie bepaalt richting en pwm waarde +{ + float waarde; + waarde = value/100; //value is waarde achter de komma, value/100 is komma getal geschikt voor pwm + + switch (c) { + case '0': + stuurmotors(0, 0, 0, 0); + break; + case '1': + stuurmotors(waarde, 0, waarde, 0); + break; + case '2': + stuurmotors(0, waarde, 0, waarde); + break; + case '3': + stuurmotors(0, waarde, waarde, 0); + break; + case '4': + stuurmotors(waarde, 0, 0, waarde); + break; + case '5': + write_wifi(temp); + break; + case '6': + write_wifi(afstand); + break; + default: + stuurmotors(0, 0, 0, 0); + write_wifi("ERROR!"); + } +} + +void wifi_config() //wifi_config schrijft commando's in wifly om adhoc op te zetten +{ + send("$$$"); //commando om wifly in command mode te zetten + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + + send("set w j 4\r"); //set wlan join 4 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set w s Edu_Robot\r"); //set wlan ssid EDU_robot + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set w c 2\r"); //set wlan channel 0 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set i a 169.254.1.1\r"); //set ip adress 169.254.1.1 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set i n 255.255.0.0\r"); //set ip netmask 255.255.0.0 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set i d 0\r"); //set ip dhcp 0 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set c c 0\r"); //set command close * + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set c o 0\r"); //set command open 0 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("set c r 0\r"); //set command remote 0 + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("save\r"); //save settings + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + + send("reboot\r"); //reboot module + pc.putc(wifi.getc()); //print acknowledge + pc.putc(wifi.getc()); + pc.putc(wifi.getc()); + while(wifi.readable()) //print ack + pc.putc(wifi.getc()); + wait(1); +} \ No newline at end of file