![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Version 1
main.cpp
- Committer:
- po514
- Date:
- 2017-03-16
- Revision:
- 7:04dff5b8490c
- Parent:
- 6:d881a556eb9a
File content as of revision 7:04dff5b8490c:
#include "mbed.h" //Photointerrupter input pins #define I1pin D2 #define I2pin D11 #define I3pin D12 //Incremental encoder input pins #define CHA D7 #define CHB D8 //Motor Drive output pins //Mask in output byte #define L1Lpin D4 //0x01 #define L1Hpin D5 //0x02 #define L2Lpin D3 //0x04 #define L2Hpin D6 //0x08 #define L3Lpin D9 //0x10 #define L3Hpin D10 //0x20 //Mapping from sequential drive states to motor phase outputs /* State L1 L2 L3 0 H - L 1 - H L 2 L H - 3 L - H 4 - L H 5 H L - 6 - - - 7 - - - */ //Drive state to output table const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed //Phase lead to make motor spin const int8_t lead = 2; //2 for forwards, -2 for backwards volatile float revTime; volatile float revPerSec; volatile float errorPrev = 0; volatile float dutyCycle; volatile float targetSpeed; volatile float acc; volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }; //Status LED DigitalOut led1(LED1); DigitalOut led2 (LED2); DigitalOut led3 (LED3); //Photointerrupter inputs InterruptIn I1intr(I1pin); InterruptIn I2intr(I2pin); InterruptIn I3intr(I3pin); DigitalIn I1(I1pin); DigitalIn I2(I2pin); DigitalIn I3(I3pin); Ticker tick; Thread th1; Thread controlLoopThread; Timer t; Serial pc(SERIAL_TX,SERIAL_RX); //Motor Drive outputs PwmOut L1L(L1Lpin); PwmOut L1H(L1Hpin); PwmOut L2L(L2Lpin); PwmOut L2H(L2Hpin); PwmOut L3L(L3Lpin); PwmOut L3H(L3Hpin); //Set a given drive state void blinkLED2(){ while(1){ led1 = 0; wait(0.5); led1 = 1; wait(0.5); } } void blinkLED3(){ while(1){ led3 = 0; wait(0.1); led3 = 1; wait(0.1); } } void mesRot(){ led3 = !led3; float speedTime; speedTime = t.read(); revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]); timeMap[I1 + 2*I2 + 4*I3] = speedTime; } void controlLoop(void){ while(true){ float error = targetSpeed - revPerSec; float errorDer = (error - errorPrev)*20; float k = 2.0; float kd = 1; dutyCycle = k*error + kd*errorDer; errorPrev = error; //remeber error wait(0.05); } } void decrease(){ dutyCycle -= 0.05; pc.printf("current value: %f", dutyCycle); if (dutyCycle < 0.2){ dutyCycle = 1; } } void motorOut(int8_t driveState, float dutyCycle){ //float dutyCycle = 1.0f; //Lookup the output byte from the drive state. int8_t driveOut = driveTable[driveState & 0x07]; //Turn off first if (~driveOut & 0x01) L1L.write(0); // = 0 if (~driveOut & 0x02) L1H.write(dutyCycle);// = 1; if (~driveOut & 0x04) L2L.write(0); // = 0; if (~driveOut & 0x08) L2H.write(dutyCycle);// = 1; if (~driveOut & 0x10) L3L.write(0);// = 0; if (~driveOut & 0x20) L3H.write(dutyCycle);// = 1; //Then turn on if (driveOut & 0x01) L1L.write(dutyCycle);// = 1; if (driveOut & 0x02) L1H.write(0); // = 0; if (driveOut & 0x04) L2L.write(dutyCycle);// = 1; if (driveOut & 0x08) L2H.write(0);// = 0; if (driveOut & 0x10) L3L.write(dutyCycle);// = 1; if (driveOut & 0x20) L3H.write(0);// = 0; } //Convert photointerrupter inputs to a rotor state inline int8_t readRotorState(){ return stateMap[I1 + 2*I2 + 4*I3]; } //Basic synchronisation routine int8_t motorHome() { //Put the motor in drive state 0 and wait for it to stabilise motorOut(0, 1); wait(1.0); //Get the rotor state return readRotorState(); } //Main int main() { int8_t orState = 0; //Rotot offset at motor state 0 //Initialise the serial port Serial pc(SERIAL_TX, SERIAL_RX); int8_t intState = 0; int8_t intStateOld = 0; float per = 0.01f; targetSpeed = 3.0; L1L.period(per); L1H.period(per); L2L.period(per); L2H.period(per); L3L.period(per); L3H.period(per); dutyCycle = 1; pc.printf("Device on \n\r"); //Run the motor synchronisation orState = motorHome(); //orState is subtracted from future rotor state inputs to align rotor and motor states //th2.set_priority(osPriorityRealtime); //th1.start(blinkLED2); //th2.start(blinkLED3); //define interrupts I1intr.rise(&mesRot); //start photoInterrupt I2intr.rise(&mesRot); I3intr.rise(&mesRot); I1intr.fall(&mesRot); I2intr.fall(&mesRot); I3intr.fall(&mesRot); controlLoopThread.start(controlLoop); //start conrol unit thread t.start(); //tick.attach(&decrease, 10); //Poll the rotor state and set the motor outputs accordingly to spin the motor while (1) { intState = readRotorState(); if (pc.readable()){ char buffer[128]; pc.gets(buffer, 6); targetSpeed = atof(buffer); pc.printf("I got '%s'\n\r", buffer); pc.printf("Also in float '%f'\n\r", targetSpeed); orState = motorHome(); } if (intState != intStateOld) { intStateOld = intState; motorOut((intState-orState+lead+6)%6, dutyCycle); //+6 to make sure the remainder is positive } } } // hi