Bjorn Van Bockel
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