main.cpp

00001 #include "mbed.h"
00002 #include <cstdlib>
00003 
00004 Serial pc(USBTX, USBRX);
00005 char c[5] = {'0','0','0','0',0};
00006 char * itoa(int i){
00007     for(int i = 0; i < 5; i++)
00008         c[i] = '0';
00009     c[4] = 0;
00010     c[3] = i%10 + '0';
00011     c[2] = (i/10) % 10 + '0';
00012     c[1] = (i/100) % 10 + '0';
00013     c[0] = (i/1000) % 10 + '0';
00014     
00015     return c;
00016 }
00017 class Motor{
00018 
00019     DigitalOut _p1, _p2, _p3, _p4;
00020     bool _smjer_cw;
00021     int _polozaj;
00022     float _brzina;// u sekundama
00023     bool _jel_ide;
00024 
00025     Ticker ticker;
00026 
00027     void postavi(int i){
00028         switch(i){
00029             case 1:
00030                 _p1 = 1;
00031                 _p2 = _p3 = _p4 = 0;
00032                 break;
00033             case 2:
00034                 _p1 = _p2 = 1;
00035                 _p3 = _p4 = 0;
00036                 break;
00037             case 3:
00038                 _p1 = _p3 =_p4 = 0;
00039                 _p2 = 1;
00040                 break;
00041             case 4:
00042                 _p1 = _p4 = 0;
00043                 _p3 = _p2 = 1;
00044                 break;
00045             case 5:
00046                 _p1 = _p2 =_p4 = 0;
00047                 _p3 = 1;
00048                 break;
00049             case 6:
00050                 _p1 = _p2 = 0;
00051                 _p3 = _p4 = 1;
00052                 break;
00053             case 7:
00054                 _p1 = _p2 = _p3 = 0;
00055                 _p4 = 1;
00056                 break;
00057             case 8:
00058                 _p1 = _p4 = 1;
00059                 _p2 = _p3 = 0;
00060                 break;
00061         }
00062     }
00063     
00064     void ugajguli(){
00065         _polozaj = 0;
00066         //jos skontati postaviti na neki 0 poloza;
00067     }
00068 
00069 public:
00070     Motor(PinName p1, PinName p2, PinName p3, PinName p4):
00071         _p1(p1), _p2(p2), _p3(p3), _p4(p4), _smjer_cw(true), _brzina(0.6),
00072         _jel_ide(false)
00073     {
00074             ugajguli();
00075     }
00076 
00077     void cw(float time = 1000){
00078         for(int i = 1; i <= 8; i++){
00079             postavi(i);
00080             wait_us(time);
00081         }
00082          _polozaj = (_polozaj + 1) % 500;
00083     }
00084 
00085     void ccw(float time = 1000){
00086         for(int i = 7; i >= 1; i--){
00087           
00088             postavi(i);
00089             wait_us(time);
00090         }
00091         postavi(8);
00092         wait_us(time);
00093         _polozaj = (_polozaj - 1) % 500;
00094     }
00095 
00096     void napravi_korak(){
00097         if(_smjer_cw) cw();
00098         else ccw();
00099     }
00100 
00101     void postavi_cw(){_smjer_cw = true;}
00102     void postavi_ccw(){_smjer_cw = false;}
00103     void promijeni_smjer(){
00104         if(_smjer_cw) _smjer_cw = false;
00105         else _smjer_cw = true;
00106     }
00107     bool Smjer() const { return _smjer_cw;}
00108     void postavi_brzinu(float sekunde){
00109         _brzina = sekunde;
00110         if(_jel_ide){
00111             stani();
00112             kreni();
00113         }
00114     }
00115 
00116     void idi_na(int i, bool smjer = true){
00117         i %= 500;
00118         if(smjer) {
00119             while(_polozaj != i){
00120                 
00121                 cw();
00122                 wait_us(1000);
00123             }
00124         } else {
00125             while(_polozaj != i){
00126                 ccw();
00127                 wait_us(1000);
00128             }
00129         }
00130     }
00131 
00132     void idi_na_polozaj(int kut){
00133         kut%=360;
00134         char  buf[20];
00135       
00136         pc.puts(itoa(kut));
00137         int p = int(kut*1.62 + 0.5);
00138 
00139                 idi_na(p, true);
00140 
00141         
00142     }
00143 
00144     void kreni(){
00145         if(_jel_ide)
00146             ticker.detach();
00147 
00148         ticker.attach(this, &Motor::napravi_korak, _brzina);
00149         _jel_ide = true;
00150     }
00151 
00152     void stani(){
00153         if(_jel_ide)
00154             ticker.detach();
00155 
00156         _jel_ide = false;
00157     }
00158     
00159     void okreni(int kut){
00160         int p =  kut*1.62 + 0.5;
00161             while(p > 0){
00162               napravi_korak();
00163               p--;   
00164             }
00165         /*if(kut >= 0){
00166             int p =  kut*1.62 + 0.5;
00167             while(p > 0){
00168               cw();
00169               p--;   
00170             }
00171         
00172         } else {
00173             kut *= -1;
00174             
00175             int p = kut*1.62 + 0.5;
00176             while(p > 0){
00177               ccw();
00178               p--;   
00179             }
00180         }*/
00181           
00182     }
00183     
00184 };
00185 
00186 
00187 Motor motor(dp13, dp11, dp10, dp9);
00188 
00189 int uzmiBroj(){
00190     char c = pc.getc();
00191     int broj = 0;
00192     while(c >= '0' && c <= '9'){
00193         broj *= 10;
00194         broj += c - '0';
00195 
00196         c=pc.getc();
00197     }
00198     
00199    
00200     pc.putc('o');
00201     pc.putc('k');
00202      return broj;
00203 }
00204 void funkcija(){
00205     if(!pc.readable()) return;
00206     switch(pc.getc())
00207     {
00208        case'K':
00209             motor.kreni();
00210             break;
00211        case'S':
00212             motor.stani();
00213              break;
00214        case'I':
00215             int i = uzmiBroj();
00216           
00217             motor.idi_na_polozaj(i);
00218             
00219             break;
00220         case 'O':
00221             motor.okreni(uzmiBroj());
00222             break;
00223         case'P':
00224             motor.promijeni_smjer();
00225             break;
00226     }
00227 }
00228 
00229 int main(){
00230     motor.postavi_brzinu(0.01);
00231     pc.attach(funkcija);
00232     while(1){
00233         
00234          
00235     }
00236 }