bmotor.cpp

00001 #include "mbed.h"
00002 #include "bmotor.h"
00003 
00004 int pola[8][6] = {{1,0,1,0,1,1}, {0,0,0,0,1,1}, {1,1,0,0,1,1}, {1,1,0,0,0,0}, {1,1,0,1,0,1}, {0,0,0,1,0,1}, {1,0,1,1,0,1}, {1,0,1,0,0,0}};
00005 int brzina = 2400;
00006 volatile int kreni = 0;
00007 volatile int _gore = 8;
00008 volatile int _dolje = 0;
00009 int okretaja = 520;
00010 
00011 Bipolar::Bipolar() : ENA(PTB9),IN1(PTC1),IN2(PTC2),IN3(PTE21),IN4(PTE20),ENB(PTB10)
00012 
00013 {
00014     startSM();
00015     b_t.start();    
00016     b_ticker.attach_us(this, &Bipolar::ciklusSM, brzina);    
00017 }
00018 
00019 void Bipolar::goreDolje(int y) {
00020     static uint8_t i = 0;
00021     if(b_t.read_us() > brzina*0.5) {
00022         
00023         switch(i) {
00024             case 0:
00025                 ENA = pola[y][0];
00026                 i++;
00027             case 1:
00028                 IN1 = pola[y][1];
00029                 i++;
00030             case 2:
00031                 IN2 = pola[y][2];
00032                 i++;
00033             case 3:
00034                 IN3 = pola[y][3];
00035                 i++;
00036             case 4:
00037                 IN4 = pola[y][4];
00038                 i++;
00039             case 5:
00040                 ENB = pola[y][5];
00041                 i = 0;
00042             default:
00043                 i = 0;
00044         }
00045         b_t.reset();
00046     } 
00047 }
00048 
00049 void Bipolar::potez() {
00050     if(b_poz.p_gore == 1) {
00051         goreDolje(--_gore);
00052         if(_gore == 0) {
00053             _gore = 8;
00054         }
00055     }
00056     if(b_poz.p_dolje == 1) {
00057         goreDolje(_dolje++);
00058         if(_dolje == 8) {
00059             _dolje = 0;
00060         }
00061     }
00062 }
00063 
00064 void Bipolar::ciklusSM() {
00065     startSM();
00066     for(int a = 0; a < okretaja; a++) {
00067         potez();
00068         if(a == (okretaja - 1)) {
00069             b_poz.done = true;
00070         }
00071     }
00072     stopSM();
00073     kreni = 0;
00074 }
00075 
00076 void Bipolar::startSM() {
00077     ENA = 0;
00078     IN1 = 0;
00079     IN2 = 0;
00080     IN3 = 0;
00081     IN4 = 0;
00082     ENB = 0;
00083 }
00084 
00085 void Bipolar::stopSM() {
00086     ENA = 1;
00087     IN1 = 1;
00088     IN2 = 1;
00089     IN3 = 1;
00090     IN4 = 1;
00091     ENB = 1;
00092 }