tim003 tim003
/
Lv9-Grupa4-Tim003-z2
Pejović Luka Elma Gazetić
main.cpp@0:7ecda32385e2, 2014-05-12 (annotated)
- Committer:
- tim003
- Date:
- Mon May 12 18:16:25 2014 +0000
- Revision:
- 0:7ecda32385e2
PAI_LV9_Grupa4_Tim003
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tim003 | 0:7ecda32385e2 | 1 | #include "mbed.h" |
tim003 | 0:7ecda32385e2 | 2 | #include <cstdlib> |
tim003 | 0:7ecda32385e2 | 3 | |
tim003 | 0:7ecda32385e2 | 4 | Serial pc(USBTX, USBRX); |
tim003 | 0:7ecda32385e2 | 5 | char c[5] = {'0','0','0','0',0}; |
tim003 | 0:7ecda32385e2 | 6 | char * itoa(int i){ |
tim003 | 0:7ecda32385e2 | 7 | for(int i = 0; i < 5; i++) |
tim003 | 0:7ecda32385e2 | 8 | c[i] = '0'; |
tim003 | 0:7ecda32385e2 | 9 | c[4] = 0; |
tim003 | 0:7ecda32385e2 | 10 | c[3] = i%10 + '0'; |
tim003 | 0:7ecda32385e2 | 11 | c[2] = (i/10) % 10 + '0'; |
tim003 | 0:7ecda32385e2 | 12 | c[1] = (i/100) % 10 + '0'; |
tim003 | 0:7ecda32385e2 | 13 | c[0] = (i/1000) % 10 + '0'; |
tim003 | 0:7ecda32385e2 | 14 | |
tim003 | 0:7ecda32385e2 | 15 | return c; |
tim003 | 0:7ecda32385e2 | 16 | } |
tim003 | 0:7ecda32385e2 | 17 | class Motor{ |
tim003 | 0:7ecda32385e2 | 18 | |
tim003 | 0:7ecda32385e2 | 19 | DigitalOut _p1, _p2, _p3, _p4; |
tim003 | 0:7ecda32385e2 | 20 | bool _smjer_cw; |
tim003 | 0:7ecda32385e2 | 21 | int _polozaj; |
tim003 | 0:7ecda32385e2 | 22 | float _brzina;// u sekundama |
tim003 | 0:7ecda32385e2 | 23 | bool _jel_ide; |
tim003 | 0:7ecda32385e2 | 24 | |
tim003 | 0:7ecda32385e2 | 25 | Ticker ticker; |
tim003 | 0:7ecda32385e2 | 26 | |
tim003 | 0:7ecda32385e2 | 27 | void postavi(int i){ |
tim003 | 0:7ecda32385e2 | 28 | switch(i){ |
tim003 | 0:7ecda32385e2 | 29 | case 1: |
tim003 | 0:7ecda32385e2 | 30 | _p1 = 1; |
tim003 | 0:7ecda32385e2 | 31 | _p2 = _p3 = _p4 = 0; |
tim003 | 0:7ecda32385e2 | 32 | break; |
tim003 | 0:7ecda32385e2 | 33 | case 2: |
tim003 | 0:7ecda32385e2 | 34 | _p1 = _p2 = 1; |
tim003 | 0:7ecda32385e2 | 35 | _p3 = _p4 = 0; |
tim003 | 0:7ecda32385e2 | 36 | break; |
tim003 | 0:7ecda32385e2 | 37 | case 3: |
tim003 | 0:7ecda32385e2 | 38 | _p1 = _p3 =_p4 = 0; |
tim003 | 0:7ecda32385e2 | 39 | _p2 = 1; |
tim003 | 0:7ecda32385e2 | 40 | break; |
tim003 | 0:7ecda32385e2 | 41 | case 4: |
tim003 | 0:7ecda32385e2 | 42 | _p1 = _p4 = 0; |
tim003 | 0:7ecda32385e2 | 43 | _p3 = _p2 = 1; |
tim003 | 0:7ecda32385e2 | 44 | break; |
tim003 | 0:7ecda32385e2 | 45 | case 5: |
tim003 | 0:7ecda32385e2 | 46 | _p1 = _p2 =_p4 = 0; |
tim003 | 0:7ecda32385e2 | 47 | _p3 = 1; |
tim003 | 0:7ecda32385e2 | 48 | break; |
tim003 | 0:7ecda32385e2 | 49 | case 6: |
tim003 | 0:7ecda32385e2 | 50 | _p1 = _p2 = 0; |
tim003 | 0:7ecda32385e2 | 51 | _p3 = _p4 = 1; |
tim003 | 0:7ecda32385e2 | 52 | break; |
tim003 | 0:7ecda32385e2 | 53 | case 7: |
tim003 | 0:7ecda32385e2 | 54 | _p1 = _p2 = _p3 = 0; |
tim003 | 0:7ecda32385e2 | 55 | _p4 = 1; |
tim003 | 0:7ecda32385e2 | 56 | break; |
tim003 | 0:7ecda32385e2 | 57 | case 8: |
tim003 | 0:7ecda32385e2 | 58 | _p1 = _p4 = 1; |
tim003 | 0:7ecda32385e2 | 59 | _p2 = _p3 = 0; |
tim003 | 0:7ecda32385e2 | 60 | break; |
tim003 | 0:7ecda32385e2 | 61 | } |
tim003 | 0:7ecda32385e2 | 62 | } |
tim003 | 0:7ecda32385e2 | 63 | |
tim003 | 0:7ecda32385e2 | 64 | void ugajguli(){ |
tim003 | 0:7ecda32385e2 | 65 | _polozaj = 0; |
tim003 | 0:7ecda32385e2 | 66 | //jos skontati postaviti na neki 0 poloza; |
tim003 | 0:7ecda32385e2 | 67 | } |
tim003 | 0:7ecda32385e2 | 68 | |
tim003 | 0:7ecda32385e2 | 69 | public: |
tim003 | 0:7ecda32385e2 | 70 | Motor(PinName p1, PinName p2, PinName p3, PinName p4): |
tim003 | 0:7ecda32385e2 | 71 | _p1(p1), _p2(p2), _p3(p3), _p4(p4), _smjer_cw(true), _brzina(0.6), |
tim003 | 0:7ecda32385e2 | 72 | _jel_ide(false) |
tim003 | 0:7ecda32385e2 | 73 | { |
tim003 | 0:7ecda32385e2 | 74 | ugajguli(); |
tim003 | 0:7ecda32385e2 | 75 | } |
tim003 | 0:7ecda32385e2 | 76 | |
tim003 | 0:7ecda32385e2 | 77 | void cw(float time = 1000){ |
tim003 | 0:7ecda32385e2 | 78 | for(int i = 1; i <= 8; i++){ |
tim003 | 0:7ecda32385e2 | 79 | postavi(i); |
tim003 | 0:7ecda32385e2 | 80 | wait_us(time); |
tim003 | 0:7ecda32385e2 | 81 | } |
tim003 | 0:7ecda32385e2 | 82 | _polozaj = (_polozaj + 1) % 500; |
tim003 | 0:7ecda32385e2 | 83 | } |
tim003 | 0:7ecda32385e2 | 84 | |
tim003 | 0:7ecda32385e2 | 85 | void ccw(float time = 1000){ |
tim003 | 0:7ecda32385e2 | 86 | for(int i = 7; i >= 1; i--){ |
tim003 | 0:7ecda32385e2 | 87 | |
tim003 | 0:7ecda32385e2 | 88 | postavi(i); |
tim003 | 0:7ecda32385e2 | 89 | wait_us(time); |
tim003 | 0:7ecda32385e2 | 90 | } |
tim003 | 0:7ecda32385e2 | 91 | postavi(8); |
tim003 | 0:7ecda32385e2 | 92 | wait_us(time); |
tim003 | 0:7ecda32385e2 | 93 | _polozaj = (_polozaj - 1) % 500; |
tim003 | 0:7ecda32385e2 | 94 | } |
tim003 | 0:7ecda32385e2 | 95 | |
tim003 | 0:7ecda32385e2 | 96 | void napravi_korak(){ |
tim003 | 0:7ecda32385e2 | 97 | if(_smjer_cw) cw(); |
tim003 | 0:7ecda32385e2 | 98 | else ccw(); |
tim003 | 0:7ecda32385e2 | 99 | } |
tim003 | 0:7ecda32385e2 | 100 | |
tim003 | 0:7ecda32385e2 | 101 | void postavi_cw(){_smjer_cw = true;} |
tim003 | 0:7ecda32385e2 | 102 | void postavi_ccw(){_smjer_cw = false;} |
tim003 | 0:7ecda32385e2 | 103 | void promijeni_smjer(){ |
tim003 | 0:7ecda32385e2 | 104 | if(_smjer_cw) _smjer_cw = false; |
tim003 | 0:7ecda32385e2 | 105 | else _smjer_cw = true; |
tim003 | 0:7ecda32385e2 | 106 | } |
tim003 | 0:7ecda32385e2 | 107 | bool Smjer() const { return _smjer_cw;} |
tim003 | 0:7ecda32385e2 | 108 | void postavi_brzinu(float sekunde){ |
tim003 | 0:7ecda32385e2 | 109 | _brzina = sekunde; |
tim003 | 0:7ecda32385e2 | 110 | if(_jel_ide){ |
tim003 | 0:7ecda32385e2 | 111 | stani(); |
tim003 | 0:7ecda32385e2 | 112 | kreni(); |
tim003 | 0:7ecda32385e2 | 113 | } |
tim003 | 0:7ecda32385e2 | 114 | } |
tim003 | 0:7ecda32385e2 | 115 | |
tim003 | 0:7ecda32385e2 | 116 | void idi_na(int i, bool smjer = true){ |
tim003 | 0:7ecda32385e2 | 117 | i %= 500; |
tim003 | 0:7ecda32385e2 | 118 | if(smjer) { |
tim003 | 0:7ecda32385e2 | 119 | while(_polozaj != i){ |
tim003 | 0:7ecda32385e2 | 120 | |
tim003 | 0:7ecda32385e2 | 121 | cw(); |
tim003 | 0:7ecda32385e2 | 122 | wait_us(1000); |
tim003 | 0:7ecda32385e2 | 123 | } |
tim003 | 0:7ecda32385e2 | 124 | } else { |
tim003 | 0:7ecda32385e2 | 125 | while(_polozaj != i){ |
tim003 | 0:7ecda32385e2 | 126 | ccw(); |
tim003 | 0:7ecda32385e2 | 127 | wait_us(1000); |
tim003 | 0:7ecda32385e2 | 128 | } |
tim003 | 0:7ecda32385e2 | 129 | } |
tim003 | 0:7ecda32385e2 | 130 | } |
tim003 | 0:7ecda32385e2 | 131 | |
tim003 | 0:7ecda32385e2 | 132 | void idi_na_polozaj(int kut){ |
tim003 | 0:7ecda32385e2 | 133 | kut%=360; |
tim003 | 0:7ecda32385e2 | 134 | char buf[20]; |
tim003 | 0:7ecda32385e2 | 135 | |
tim003 | 0:7ecda32385e2 | 136 | pc.puts(itoa(kut)); |
tim003 | 0:7ecda32385e2 | 137 | int p = int(kut*1.62 + 0.5); |
tim003 | 0:7ecda32385e2 | 138 | |
tim003 | 0:7ecda32385e2 | 139 | idi_na(p, true); |
tim003 | 0:7ecda32385e2 | 140 | |
tim003 | 0:7ecda32385e2 | 141 | |
tim003 | 0:7ecda32385e2 | 142 | } |
tim003 | 0:7ecda32385e2 | 143 | |
tim003 | 0:7ecda32385e2 | 144 | void kreni(){ |
tim003 | 0:7ecda32385e2 | 145 | if(_jel_ide) |
tim003 | 0:7ecda32385e2 | 146 | ticker.detach(); |
tim003 | 0:7ecda32385e2 | 147 | |
tim003 | 0:7ecda32385e2 | 148 | ticker.attach(this, &Motor::napravi_korak, _brzina); |
tim003 | 0:7ecda32385e2 | 149 | _jel_ide = true; |
tim003 | 0:7ecda32385e2 | 150 | } |
tim003 | 0:7ecda32385e2 | 151 | |
tim003 | 0:7ecda32385e2 | 152 | void stani(){ |
tim003 | 0:7ecda32385e2 | 153 | if(_jel_ide) |
tim003 | 0:7ecda32385e2 | 154 | ticker.detach(); |
tim003 | 0:7ecda32385e2 | 155 | |
tim003 | 0:7ecda32385e2 | 156 | _jel_ide = false; |
tim003 | 0:7ecda32385e2 | 157 | } |
tim003 | 0:7ecda32385e2 | 158 | |
tim003 | 0:7ecda32385e2 | 159 | void okreni(int kut){ |
tim003 | 0:7ecda32385e2 | 160 | int p = kut*1.62 + 0.5; |
tim003 | 0:7ecda32385e2 | 161 | while(p > 0){ |
tim003 | 0:7ecda32385e2 | 162 | napravi_korak(); |
tim003 | 0:7ecda32385e2 | 163 | p--; |
tim003 | 0:7ecda32385e2 | 164 | } |
tim003 | 0:7ecda32385e2 | 165 | /*if(kut >= 0){ |
tim003 | 0:7ecda32385e2 | 166 | int p = kut*1.62 + 0.5; |
tim003 | 0:7ecda32385e2 | 167 | while(p > 0){ |
tim003 | 0:7ecda32385e2 | 168 | cw(); |
tim003 | 0:7ecda32385e2 | 169 | p--; |
tim003 | 0:7ecda32385e2 | 170 | } |
tim003 | 0:7ecda32385e2 | 171 | |
tim003 | 0:7ecda32385e2 | 172 | } else { |
tim003 | 0:7ecda32385e2 | 173 | kut *= -1; |
tim003 | 0:7ecda32385e2 | 174 | |
tim003 | 0:7ecda32385e2 | 175 | int p = kut*1.62 + 0.5; |
tim003 | 0:7ecda32385e2 | 176 | while(p > 0){ |
tim003 | 0:7ecda32385e2 | 177 | ccw(); |
tim003 | 0:7ecda32385e2 | 178 | p--; |
tim003 | 0:7ecda32385e2 | 179 | } |
tim003 | 0:7ecda32385e2 | 180 | }*/ |
tim003 | 0:7ecda32385e2 | 181 | |
tim003 | 0:7ecda32385e2 | 182 | } |
tim003 | 0:7ecda32385e2 | 183 | |
tim003 | 0:7ecda32385e2 | 184 | }; |
tim003 | 0:7ecda32385e2 | 185 | |
tim003 | 0:7ecda32385e2 | 186 | |
tim003 | 0:7ecda32385e2 | 187 | Motor motor(dp13, dp11, dp10, dp9); |
tim003 | 0:7ecda32385e2 | 188 | |
tim003 | 0:7ecda32385e2 | 189 | int uzmiBroj(){ |
tim003 | 0:7ecda32385e2 | 190 | char c = pc.getc(); |
tim003 | 0:7ecda32385e2 | 191 | int broj = 0; |
tim003 | 0:7ecda32385e2 | 192 | while(c >= '0' && c <= '9'){ |
tim003 | 0:7ecda32385e2 | 193 | broj *= 10; |
tim003 | 0:7ecda32385e2 | 194 | broj += c - '0'; |
tim003 | 0:7ecda32385e2 | 195 | |
tim003 | 0:7ecda32385e2 | 196 | c=pc.getc(); |
tim003 | 0:7ecda32385e2 | 197 | } |
tim003 | 0:7ecda32385e2 | 198 | |
tim003 | 0:7ecda32385e2 | 199 | |
tim003 | 0:7ecda32385e2 | 200 | pc.putc('o'); |
tim003 | 0:7ecda32385e2 | 201 | pc.putc('k'); |
tim003 | 0:7ecda32385e2 | 202 | return broj; |
tim003 | 0:7ecda32385e2 | 203 | } |
tim003 | 0:7ecda32385e2 | 204 | void funkcija(){ |
tim003 | 0:7ecda32385e2 | 205 | if(!pc.readable()) return; |
tim003 | 0:7ecda32385e2 | 206 | switch(pc.getc()) |
tim003 | 0:7ecda32385e2 | 207 | { |
tim003 | 0:7ecda32385e2 | 208 | case'K': |
tim003 | 0:7ecda32385e2 | 209 | motor.kreni(); |
tim003 | 0:7ecda32385e2 | 210 | break; |
tim003 | 0:7ecda32385e2 | 211 | case'S': |
tim003 | 0:7ecda32385e2 | 212 | motor.stani(); |
tim003 | 0:7ecda32385e2 | 213 | break; |
tim003 | 0:7ecda32385e2 | 214 | case'I': |
tim003 | 0:7ecda32385e2 | 215 | int i = uzmiBroj(); |
tim003 | 0:7ecda32385e2 | 216 | |
tim003 | 0:7ecda32385e2 | 217 | motor.idi_na_polozaj(i); |
tim003 | 0:7ecda32385e2 | 218 | |
tim003 | 0:7ecda32385e2 | 219 | break; |
tim003 | 0:7ecda32385e2 | 220 | case 'O': |
tim003 | 0:7ecda32385e2 | 221 | motor.okreni(uzmiBroj()); |
tim003 | 0:7ecda32385e2 | 222 | break; |
tim003 | 0:7ecda32385e2 | 223 | case'P': |
tim003 | 0:7ecda32385e2 | 224 | motor.promijeni_smjer(); |
tim003 | 0:7ecda32385e2 | 225 | break; |
tim003 | 0:7ecda32385e2 | 226 | } |
tim003 | 0:7ecda32385e2 | 227 | } |
tim003 | 0:7ecda32385e2 | 228 | |
tim003 | 0:7ecda32385e2 | 229 | int main(){ |
tim003 | 0:7ecda32385e2 | 230 | motor.postavi_brzinu(0.01); |
tim003 | 0:7ecda32385e2 | 231 | pc.attach(funkcija); |
tim003 | 0:7ecda32385e2 | 232 | while(1){ |
tim003 | 0:7ecda32385e2 | 233 | |
tim003 | 0:7ecda32385e2 | 234 | |
tim003 | 0:7ecda32385e2 | 235 | } |
tim003 | 0:7ecda32385e2 | 236 | } |