Embedded Systems
Version 1
Diff: main.cpp
- Revision:
- 0:bb40c446eb83
- Child:
- 1:05d69ef9c7e4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Mar 16 11:30:20 2017 +0000
@@ -0,0 +1,194 @@
+#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 revPerMin;
+volatile float freq;
+
+//Status LED
+DigitalOut led1(LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+
+//Photointerrupter inputs
+InterruptIn I1intr(I1pin);
+DigitalIn I1(I1pin);
+DigitalIn I2(I2pin);
+DigitalIn I3(I3pin);
+
+Ticker tick;
+Thread th1;
+Thread th2;
+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 toggleLED(){
+ led3 = !led3;
+ float speedTime;
+ speedTime = t.read();
+ revPerMin = 1.0/(speedTime - revTime);
+ revTime = speedTime;
+ pc.printf("speed: %f \n\r", revPerMin);
+}
+
+void decrease(){
+ freq -= 0.05;
+ pc.printf("current value: %f", freq);
+ if (freq < 0.2){
+ freq = 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;
+
+ L1L.period(per);
+ L1H.period(per);
+ L2L.period(per);
+ L2H.period(per);
+ L3L.period(per);
+ L3H.period(per);
+ freq = 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);
+ I1intr.rise(&toggleLED);
+ 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);
+ freq = atof(buffer);
+ pc.printf("I got '%s'\n\r", buffer);
+ pc.printf("Also in float '%f'\n\r", freq);
+ orState = motorHome();
+ }
+
+ if (intState != intStateOld) {
+ intStateOld = intState;
+ motorOut((intState-orState+lead+6)%6, freq); //+6 to make sure the remainder is positive
+ }
+ }
+}
+
+