![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
An embedded device
Dependencies: Crypto
Diff: main.cpp
- Revision:
- 2:4e88faab6988
- Parent:
- 1:184cb0870c04
- Child:
- 3:569b35e2a602
diff -r 184cb0870c04 -r 4e88faab6988 main.cpp --- a/main.cpp Mon Feb 27 10:25:06 2017 +0000 +++ b/main.cpp Tue Feb 28 14:44:23 2017 +0000 @@ -3,15 +3,19 @@ //Photointerrupter input pins #define I1pin D2 -#define I2pin D3 -#define I3pin D4 +#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 PA_6 //0x01 -#define L1Hpin D6 //0x02 -#define L2Lpin PA_7 //0x04 -#define L2Hpin D8 //0x08 -#define L3Lpin PA_4 //0x10 +#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 @@ -28,18 +32,21 @@ */ //Drive state to output table const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; -//Mask to invert the outputs for high side transistors -const int8_t motorHmask = 0x2a; + //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,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 //Status LED DigitalOut led1(LED1); //Photointerrupter inputs -DigitalIn I1int(I1pin); -DigitalIn I2int(I2pin); -DigitalIn I3int(I3pin); +DigitalIn I1(I1pin); +DigitalIn I2(I2pin); +DigitalIn I3(I3pin); //Motor Drive outputs DigitalOut L1L(L1Lpin); @@ -49,59 +56,66 @@ DigitalOut L3L(L3Lpin); DigitalOut L3H(L3Hpin); -//Global state -int32_t outState = 0; //Current drive state -int32_t inState = 0; //Current rotor state -int32_t leadState = 0; //Offset to calculate drive state from rotor state - //Set a given drive state void motorOut(int8_t driveState){ - //First turn all phases off to prevent shoot-through - L1H = 1; - L2H = 1; - L3H = 1; - - //Lookup the output byte from the drive state. Apply high side inversion mask - int8_t driveOut = driveTable[driveState & 0x07] ^ motorHmask; - //Apply the output byte to the pins - L1L = driveOut & 0x01; - L2L = driveOut & 0x04; - L3L = driveOut & 0x10; - L1H = driveOut & 0x02; - L2H = driveOut & 0x08; - L3H = driveOut & 0x20; + //Lookup the output byte from the drive state. + int8_t driveOut = driveTable[driveState & 0x07]; + + //Turn off first + if (~driveOut & 0x01) L1L = 0; + if (~driveOut & 0x02) L1H = 1; + if (~driveOut & 0x04) L2L = 0; + if (~driveOut & 0x08) L2H = 1; + if (~driveOut & 0x10) L3L = 0; + if (~driveOut & 0x20) L3H = 1; + + //Then turn on + if (driveOut & 0x01) L1L = 1; + if (driveOut & 0x02) L1H = 0; + if (driveOut & 0x04) L2L = 1; + if (driveOut & 0x08) L2H = 0; + if (driveOut & 0x10) L3L = 1; + if (driveOut & 0x20) L3H = 0; } + //Convert photointerrupter inputs to a rotor state inline int8_t readRotorState(){ - return stateMap[I1int + 2*I2int + 4*I3int]; + return stateMap[I1 + 2*I2 + 4*I3]; } //Basic synchronisation routine -void motorHome() { +int8_t motorHome() { //Put the motor in drive state 0 and wait for it to stabilise motorOut(0); wait(1.0); //Get the rotor state - inState = readRotorState(); - - //Calculate the rotor-to-drive offset by adding two (modulo 6) the current rotor state - leadState = (inState+2)%6; + 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; pc.printf("Hello\n\r"); //Run the motor synchronisation - motorHome(); + orState = motorHome(); + pc.printf("Rotor origin: %x\n\r",orState); + //orState is subtracted from future rotor state inputs to align rotor and motor states //Poll the rotor state and set the motor outputs accordingly to spin the motor while (1) { - motorOut(readRotorState()+leadState); + intState = readRotorState(); + if (intState != intStateOld) { + intStateOld = intState; + motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive } + } }