main.cpp

00001 #include "mbed.h"
00002 
00003 //Photointerrupter input pins
00004 #define I1pin D2
00005 #define I2pin D11
00006 #define I3pin D12
00007 
00008 //Incremental encoder input pins
00009 #define CHA   D7
00010 #define CHB   D8  
00011 
00012 //Motor Drive output pins   //Mask in output byte
00013 #define L1Lpin D4           //0x01
00014 #define L1Hpin D5           //0x02
00015 #define L2Lpin D3           //0x04
00016 #define L2Hpin D6           //0x08
00017 #define L3Lpin D9           //0x10
00018 #define L3Hpin D10          //0x20
00019 
00020 //Mapping from sequential drive states to motor phase outputs
00021 /*
00022 State   L1  L2  L3
00023 0       H   -   L
00024 1       -   H   L
00025 2       L   H   -
00026 3       L   -   H
00027 4       -   L   H
00028 5       H   L   -
00029 6       -   -   -
00030 7       -   -   -
00031 */
00032 //Drive state to output table
00033 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
00034 
00035 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
00036 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};  
00037 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
00038 
00039 //Phase lead to make motor spin
00040 const int8_t lead = 2;  //2 for forwards, -2 for backwards
00041 
00042 volatile float revTime;
00043 volatile float revPerSec;
00044 volatile float errorPrev = 0;
00045 volatile float dutyCycle;
00046 volatile float targetSpeed;
00047 volatile float acc;
00048 
00049 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
00050 
00051 //Status LED
00052 DigitalOut led1(LED1);
00053 DigitalOut led2 (LED2);
00054 DigitalOut led3 (LED3);
00055 
00056 //Photointerrupter inputs
00057 InterruptIn I1intr(I1pin);
00058 InterruptIn I2intr(I2pin);
00059 InterruptIn I3intr(I3pin);
00060 
00061 DigitalIn I1(I1pin);
00062 DigitalIn I2(I2pin);
00063 DigitalIn I3(I3pin);
00064 
00065 Ticker tick;
00066 Thread th1;
00067 Thread controlLoopThread;
00068 Timer t;
00069 
00070 Serial pc(SERIAL_TX,SERIAL_RX);
00071 //Motor Drive outputs
00072 PwmOut L1L(L1Lpin);
00073 PwmOut L1H(L1Hpin);
00074 PwmOut L2L(L2Lpin);
00075 PwmOut L2H(L2Hpin);
00076 PwmOut L3L(L3Lpin);
00077 PwmOut L3H(L3Hpin);
00078 
00079 //Set a given drive state
00080 
00081 void blinkLED2(){
00082     while(1){
00083         led1 = 0;   
00084         wait(0.5);
00085         led1 = 1;
00086         wait(0.5);
00087     }
00088 }
00089 
00090 void blinkLED3(){
00091     while(1){
00092         led3 = 0;   
00093         wait(0.1);
00094         led3 = 1;
00095         wait(0.1);
00096     }
00097 }
00098 
00099 void mesRot(){
00100     led3 = !led3;
00101     float speedTime;
00102     speedTime = t.read();
00103     revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
00104     timeMap[I1 + 2*I2 + 4*I3] = speedTime;
00105 }   
00106 
00107 void controlLoop(void){
00108     while(true){
00109         float error = targetSpeed - revPerSec;
00110         float errorDer = (error - errorPrev)*20;
00111         float k = 2.0;
00112         float kd = 1;
00113         dutyCycle = k*error + kd*errorDer;
00114         
00115         errorPrev = error; //remeber error
00116         wait(0.05);
00117     }
00118 }
00119 
00120 void decrease(){
00121     dutyCycle -= 0.05;
00122     pc.printf("current value: %f", dutyCycle);
00123     if (dutyCycle < 0.2){
00124         dutyCycle = 1;
00125         }    
00126 }
00127         
00128     
00129 void motorOut(int8_t driveState, float dutyCycle){
00130     //float dutyCycle = 1.0f;
00131     //Lookup the output byte from the drive state.
00132     int8_t driveOut = driveTable[driveState & 0x07];
00133       
00134     //Turn off first
00135     if (~driveOut & 0x01) L1L.write(0); // = 0
00136     if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1;
00137     if (~driveOut & 0x04) L2L.write(0); // = 0;
00138     if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1;
00139     if (~driveOut & 0x10) L3L.write(0);// = 0;
00140     if (~driveOut & 0x20) L3H.write(dutyCycle);// = 1;
00141     
00142     //Then turn on
00143     if (driveOut & 0x01) L1L.write(dutyCycle);// = 1;
00144     if (driveOut & 0x02) L1H.write(0); // = 0;
00145     if (driveOut & 0x04) L2L.write(dutyCycle);// = 1;
00146     if (driveOut & 0x08) L2H.write(0);// = 0;
00147     if (driveOut & 0x10) L3L.write(dutyCycle);// = 1;
00148     if (driveOut & 0x20) L3H.write(0);// = 0;
00149     }
00150     
00151     //Convert photointerrupter inputs to a rotor state
00152 inline int8_t readRotorState(){
00153     return stateMap[I1 + 2*I2 + 4*I3];
00154     }
00155 
00156 //Basic synchronisation routine    
00157 int8_t motorHome() {
00158     //Put the motor in drive state 0 and wait for it to stabilise
00159     motorOut(0, 1);
00160     wait(1.0);
00161     
00162     //Get the rotor state
00163     return readRotorState();
00164 }
00165     
00166 //Main
00167 int main() {
00168     int8_t orState = 0;    //Rotot offset at motor state 0
00169     
00170     //Initialise the serial port
00171     Serial pc(SERIAL_TX, SERIAL_RX);
00172     int8_t intState = 0;
00173     int8_t intStateOld = 0;
00174     float per = 0.01f; 
00175     targetSpeed = 3.0;
00176     
00177     L1L.period(per);
00178     L1H.period(per);
00179     L2L.period(per);
00180     L2H.period(per);
00181     L3L.period(per);
00182     L3H.period(per);
00183     dutyCycle = 1;
00184     pc.printf("Device on \n\r");
00185     
00186     //Run the motor synchronisation
00187     orState = motorHome();
00188     //orState is subtracted from future rotor state inputs to align rotor and motor states
00189     //th2.set_priority(osPriorityRealtime);
00190     //th1.start(blinkLED2);
00191     //th2.start(blinkLED3);
00192     
00193     //define interrupts
00194     I1intr.rise(&mesRot); //start photoInterrupt
00195     I2intr.rise(&mesRot);
00196     I3intr.rise(&mesRot);
00197     I1intr.fall(&mesRot);
00198     I2intr.fall(&mesRot);
00199     I3intr.fall(&mesRot);
00200     
00201     controlLoopThread.start(controlLoop);  //start conrol unit thread
00202     t.start();
00203     //tick.attach(&decrease, 10);
00204     //Poll the rotor state and set the motor outputs accordingly to spin the motor
00205     while (1) {
00206         intState = readRotorState();
00207         if (pc.readable()){
00208              char buffer[128];
00209         
00210                 pc.gets(buffer, 6);
00211                 targetSpeed = atof(buffer);
00212                 pc.printf("I got '%s'\n\r", buffer); 
00213                 pc.printf("Also in float '%f'\n\r", targetSpeed); 
00214                 orState = motorHome();
00215             }
00216         
00217         if (intState != intStateOld) {
00218             intStateOld = intState;
00219             motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive
00220         }
00221     }
00222 }
00223 
00224 
00225 // hi