Diff: main.cpp

Revision:

0:f3a3f80e3202

Child:

1:1e01b12063cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,243 @@
+//*****biblioteki*****
+
+#include "mbed.h"
+#include "Engine.h"
+#include "Servo.h"
+#include "HCSR04.h"
+
+//*****piny*****
+
+Serial stm(PA_2, PA_3);
+Servo cam_horizontal(PB_6);
+Servo cam_vertical(PC_7);
+Engine eng_left = Engine(PB_13, PB_4, PB_10);
+Engine eng_right = Engine(PB_14, PB_5, PB_3);
+HCSR04 sensor(PB_9, PB_8, 11770);
+
+//*****deklaracja zmiennych*****
+
+const int BufferSize=15;
+char buffer1[BufferSize];
+char buffer2[BufferSize];
+char camove[2];
+int wall=30;        // odleglosc od sciany w cm
+int act_vertical=0, act_horizontal=0, move=0, val=0;
+Timer t, t_sonar;
+long distance;
+volatile unsigned int speed_car = 0;
+
+//*****funkcje*****
+
+void cleanBuffer1()             // czyszczaca bufor
+{
+    for(int i=0; i<=BufferSize; i++)
+        buffer1[i]=NULL;
+}
+
+void cleanBuffer2()             // czyszczaca bufor2
+{
+    for(int i=0; i<=BufferSize; i++)
+        buffer2[i]=NULL;
+}
+
+int what_command(char* buffer1){             // rozpoznajaca komende
+    if ((buffer1[0]=='f') && (distance>30))
+        return 1;
+    else if ((buffer1[0]=='t') && (distance>30))
+        if (buffer1[2]=='l')
+            return 2;
+        else
+            return 3;
+    else if (buffer1[0]=='b')
+        return 4;
+    else if (buffer1[0]=='S')
+        return 5;
+    else if (buffer1[0]=='l')
+        return 6;
+    else if (buffer1[0]=='r')
+        return 7;
+    else if (buffer1[0]=='v')
+        if (buffer1[4]=='l')
+            return 8;
+        else
+            return 10;
+    else if (buffer1[0]=='h')
+        if (buffer1[4]=='l')
+            return 9;
+        else
+            return 11;
+    else if (buffer1[0]=='_')
+        return 12;
+    else
+        return 0;
+}
+
+void hard_stop()             // zatrzymujaca robota
+{
+    eng_left.move(0);
+    eng_right.move(0); 
+}
+
+int map(int value, int from_min, int from_max, int to_min, int to_max)             // mapujaca wartosc do skretu
+{
+    return (value - from_min) * (to_max - to_min)/(from_max - from_min) + to_min;
+}
+
+void move_camera_down(int val)             // sterujaca kamera
+{
+    act_vertical = act_vertical + val;
+    if(act_vertical>90){
+        act_vertical=90;
+    }
+    cam_vertical.position(act_vertical);
+}
+
+void move_camera_up(int val)             // sterujaca kamera
+{
+    act_vertical = act_vertical - val;
+    if(act_vertical<-90){
+        act_vertical=-90;
+    }
+    cam_vertical.position(act_vertical);
+}
+
+void move_camera_right(int val)             // sterujaca kamera
+{
+    act_horizontal = act_horizontal - val;
+    if(act_horizontal<-90){
+        act_horizontal=-90;
+    }
+    cam_horizontal.position(act_horizontal);
+}
+
+void move_camera_left(int val)             // sterujaca kamera
+{
+    act_horizontal = act_horizontal + val;
+    if(act_horizontal>90){
+        act_horizontal=90;
+    }
+    cam_horizontal.position(act_horizontal);
+}
+
+void set_camera_zero()             // zerujaca kamere
+{
+     cam_horizontal.position(-5);
+     cam_vertical.position(-20);       
+}
+
+void set_val()          // odczytująca wartosc
+{
+    for (int q=0; q<3; q++){
+        buffer2[q]=buffer1[q+8];
+        }
+        sscanf(buffer2, "%3d", &val);
+}
+
+int main(){
+    stm.baud(115200);
+    cleanBuffer1();
+    cleanBuffer2(); 
+    set_camera_zero();
+    t_sonar.start();
+    
+    while(true){
+        stm.printf("Poczatek while \n");
+        if(t.read_ms()>500)         //jesli przez ponad 500ms nie ma nowej ramki, zatrzymujemy robota
+            { 
+            eng_left.move(0);
+            eng_right.move(0);
+            }
+        if(t_sonar.read_ms()>1000)          //odczyt odleglosci co 1,2s
+        {
+            distance = sensor.distance();
+            stm.printf("%d\n",distance);
+            
+            t_sonar.stop();
+            t_sonar.reset();
+            t_sonar.start();
+        }
+        while(stm.readable()){
+            stm.gets(buffer1, 13);
+            break;}
+        
+        stm.printf("bufor pierwszy: \n");
+        
+        for (int f=0; f<20; f++)
+        {
+            stm.printf("%c", buffer1[f]);
+        }
+        
+        stm.printf("\n");
+        
+        set_val();
+        
+        stm.printf("%i", val);
+        
+        t.stop();
+        t.reset();
+        t.start();
+        switch (what_command(buffer1))
+        {
+            case 1:
+                eng_left.move(val);
+                eng_right.move(val); 
+                set_camera_zero();
+                speed_car = val;
+                break;
+            case 2:
+                eng_right.move(speed_car);
+                eng_left.move(speed_car - map(val, 0, 255, 0, speed_car));
+                cam_horizontal.position(15);
+                cam_vertical.position(0);
+                break;
+            case 3:
+                eng_left.move(speed_car);
+                eng_right.move(speed_car - map(val, 0, 255, 0, speed_car));
+                cam_horizontal.position(0);
+                cam_vertical.position(-35);
+                break;
+            case 4:
+                eng_left.move(-val);
+                eng_right.move(-val); 
+                set_camera_zero();
+                speed_car = val;
+                break;
+            case 5:
+                hard_stop();
+                speed_car = 0;
+                break;
+            case 6:
+                eng_left.move(-val);
+                eng_right.move(val); 
+                break;
+            case 7:
+                eng_left.move(val);
+                eng_right.move(-val);
+                break;
+            case 8:
+                move_camera_down(val);
+                break;
+            case 9:
+                move_camera_left(val);
+                break;
+            case 10:
+                move_camera_up(val);
+                break;
+            case 11:
+                move_camera_right(val);
+                break;
+            case 12:
+                set_camera_zero();  
+                break;
+            default:
+                stm.printf("unknown_command \n");
+                break;
+        }
+        cleanBuffer1();
+        cleanBuffer2();
+        
+        stm.printf("Koniec while \n");
+        
+        wait_ms(2000);
+    }
+}
\ No newline at end of file