Revision 0:f3a3f80e3202, committed 2016-07-07

Comitter:

yruiewyrui3

Date:

Thu Jul 07 12:56:54 2016 +0000

Commit message:

Jak Krisowy

Changed in this revision

diff -r 000000000000 -r f3a3f80e3202 Engine.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Engine.cpp	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,44 @@
+#include "Engine.h"
+
+// przelicza procenty na pwn:
+// - wartosc poczatkowa - 0.2377,
+// - prog - 0.0077
+float Engine::getPwm(float speed){
+    if (speed == 0)
+        return 0;
+    else if (speed > 0)
+        return 0.2377f + (fabs(speed) - 1) * 0.0077f;
+    else
+        return 0.406f + (fabs(speed) - 1) * 0.006f;
+    }
+
+void Engine::move(int speed){
+    if (speed > 100)
+        speed =100;
+    else if (speed < -100)
+        speed = -100;
+    
+    if (speed == 0){
+        //soft stop
+        _fwd = 0;
+        _rev = 0;
+        return;
+        }
+            
+    if (speed < 0){
+        _fwd = 0;
+        _rev = 1;
+        }
+        
+    else{
+        _fwd = 1;
+        _rev = 0;
+        }
+    
+    _pwm = getPwm(speed);
+}
+
+//hard stop
+void Engine::stop(){
+    _pwm = 0;
+}
\ No newline at end of file

diff -r 000000000000 -r f3a3f80e3202 Engine.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Engine.h	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,22 @@
+#ifndef ENGINE_H
+#define ENGINE_H
+
+#include "mbed.h"
+
+
+class Engine{
+  private:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+    
+    float getPwm(float val);
+    
+  public:
+    Engine(PinName pwm, PinName fwd, PinName rev): _pwm(pwm), _fwd(fwd), _rev(rev){}
+    void move(int speed); // silniki pracuja z okreslona predkoscia (speed ma zakres od -1 do 1)
+    void stop();    //hard stop
+};
+
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r f3a3f80e3202 HCSR04.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yruiewyrui3/code/HCSR/#01ecd819e4ee

diff -r 000000000000 -r f3a3f80e3202 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yruiewyrui3/code/Ser/#fc2e5d57af37

diff -r 000000000000 -r f3a3f80e3202 main.cpp
--- /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

diff -r 000000000000 -r f3a3f80e3202 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Jul 07 12:56:54 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6c34061e7c34
\ No newline at end of file