code voor edu_robot
Dependencies: LM75B SRF02 mbed
main.cpp
- Committer:
- BjornVB
- Date:
- 2014-02-26
- Revision:
- 0:4b909635e2d2
File content as of revision 0:4b909635e2d2:
#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); }