Diff: main.cpp

Revision:

0:7ecda32385e2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 12 18:16:25 2014 +0000
@@ -0,0 +1,236 @@
+#include "mbed.h"
+#include <cstdlib>
+
+Serial pc(USBTX, USBRX);
+char c[5] = {'0','0','0','0',0};
+char * itoa(int i){
+    for(int i = 0; i < 5; i++)
+        c[i] = '0';
+    c[4] = 0;
+    c[3] = i%10 + '0';
+    c[2] = (i/10) % 10 + '0';
+    c[1] = (i/100) % 10 + '0';
+    c[0] = (i/1000) % 10 + '0';
+    
+    return c;
+}
+class Motor{
+
+    DigitalOut _p1, _p2, _p3, _p4;
+    bool _smjer_cw;
+    int _polozaj;
+    float _brzina;// u sekundama
+    bool _jel_ide;
+
+    Ticker ticker;
+
+    void postavi(int i){
+        switch(i){
+            case 1:
+                _p1 = 1;
+                _p2 = _p3 = _p4 = 0;
+                break;
+            case 2:
+                _p1 = _p2 = 1;
+                _p3 = _p4 = 0;
+                break;
+            case 3:
+                _p1 = _p3 =_p4 = 0;
+                _p2 = 1;
+                break;
+            case 4:
+                _p1 = _p4 = 0;
+                _p3 = _p2 = 1;
+                break;
+            case 5:
+                _p1 = _p2 =_p4 = 0;
+                _p3 = 1;
+                break;
+            case 6:
+                _p1 = _p2 = 0;
+                _p3 = _p4 = 1;
+                break;
+            case 7:
+                _p1 = _p2 = _p3 = 0;
+                _p4 = 1;
+                break;
+            case 8:
+                _p1 = _p4 = 1;
+                _p2 = _p3 = 0;
+                break;
+        }
+    }
+    
+    void ugajguli(){
+        _polozaj = 0;
+        //jos skontati postaviti na neki 0 poloza;
+    }
+
+public:
+    Motor(PinName p1, PinName p2, PinName p3, PinName p4):
+        _p1(p1), _p2(p2), _p3(p3), _p4(p4), _smjer_cw(true), _brzina(0.6),
+        _jel_ide(false)
+    {
+            ugajguli();
+    }
+
+    void cw(float time = 1000){
+        for(int i = 1; i <= 8; i++){
+            postavi(i);
+            wait_us(time);
+        }
+         _polozaj = (_polozaj + 1) % 500;
+    }
+
+    void ccw(float time = 1000){
+        for(int i = 7; i >= 1; i--){
+          
+            postavi(i);
+            wait_us(time);
+        }
+        postavi(8);
+        wait_us(time);
+        _polozaj = (_polozaj - 1) % 500;
+    }
+
+    void napravi_korak(){
+        if(_smjer_cw) cw();
+        else ccw();
+    }
+
+    void postavi_cw(){_smjer_cw = true;}
+    void postavi_ccw(){_smjer_cw = false;}
+    void promijeni_smjer(){
+        if(_smjer_cw) _smjer_cw = false;
+        else _smjer_cw = true;
+    }
+    bool Smjer() const { return _smjer_cw;}
+    void postavi_brzinu(float sekunde){
+        _brzina = sekunde;
+        if(_jel_ide){
+            stani();
+            kreni();
+        }
+    }
+
+    void idi_na(int i, bool smjer = true){
+        i %= 500;
+        if(smjer) {
+            while(_polozaj != i){
+                
+                cw();
+                wait_us(1000);
+            }
+        } else {
+            while(_polozaj != i){
+                ccw();
+                wait_us(1000);
+            }
+        }
+    }
+
+    void idi_na_polozaj(int kut){
+        kut%=360;
+        char  buf[20];
+      
+        pc.puts(itoa(kut));
+        int p = int(kut*1.62 + 0.5);
+
+                idi_na(p, true);
+
+        
+    }
+
+    void kreni(){
+        if(_jel_ide)
+            ticker.detach();
+
+        ticker.attach(this, &Motor::napravi_korak, _brzina);
+        _jel_ide = true;
+    }
+
+    void stani(){
+        if(_jel_ide)
+            ticker.detach();
+
+        _jel_ide = false;
+    }
+    
+    void okreni(int kut){
+        int p =  kut*1.62 + 0.5;
+            while(p > 0){
+              napravi_korak();
+              p--;   
+            }
+        /*if(kut >= 0){
+            int p =  kut*1.62 + 0.5;
+            while(p > 0){
+              cw();
+              p--;   
+            }
+        
+        } else {
+            kut *= -1;
+            
+            int p = kut*1.62 + 0.5;
+            while(p > 0){
+              ccw();
+              p--;   
+            }
+        }*/
+          
+    }
+    
+};
+
+
+Motor motor(dp13, dp11, dp10, dp9);
+
+int uzmiBroj(){
+    char c = pc.getc();
+    int broj = 0;
+    while(c >= '0' && c <= '9'){
+        broj *= 10;
+        broj += c - '0';
+
+        c=pc.getc();
+    }
+    
+   
+    pc.putc('o');
+    pc.putc('k');
+     return broj;
+}
+void funkcija(){
+    if(!pc.readable()) return;
+    switch(pc.getc())
+    {
+       case'K':
+            motor.kreni();
+            break;
+       case'S':
+            motor.stani();
+             break;
+       case'I':
+            int i = uzmiBroj();
+          
+            motor.idi_na_polozaj(i);
+            
+            break;
+        case 'O':
+            motor.okreni(uzmiBroj());
+            break;
+        case'P':
+            motor.promijeni_smjer();
+            break;
+    }
+}
+
+int main(){
+    motor.postavi_brzinu(0.01);
+    pc.attach(funkcija);
+    while(1){
+        
+         
+    }
+}