Revision 3:620fbdd7b6c6, committed 2017-03-24

Comitter:

Bachoru

Date:

Fri Mar 24 00:48:47 2017 +0000

Parent:

2:4e88faab6988

Commit message:

New stuff

Changed in this revision

diff -r 4e88faab6988 -r 620fbdd7b6c6 main.cpp
--- a/main.cpp	Tue Feb 28 14:44:23 2017 +0000
+++ b/main.cpp	Fri Mar 24 00:48:47 2017 +0000
@@ -1,15 +1,37 @@
 #include "mbed.h"
-#include "rtos.h"
+
+/////////////////////
+// Pin definitions //
+/////////////////////
 
 //Photointerrupter input pins
 #define I1pin D2
 #define I2pin D11
 #define I3pin D12
 
+DigitalIn I1(I1pin);
+DigitalIn I2(I2pin);
+DigitalIn I3(I3pin);
+
+InterruptIn I1intr(I1pin);
+InterruptIn I2intr(I2pin);
+InterruptIn I3intr(I3pin);
+
 //Incremental encoder input pins
 #define CHA   D7
 #define CHB   D8  
 
+DigitalIn CHAR(CHA);
+DigitalIn CHBR(CHB);
+
+InterruptIn CHAintr(CHA);
+InterruptIn CHBintr(CHB);
+
+//Status LED
+DigitalOut led1 (LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+
 //Motor Drive output pins   //Mask in output byte
 #define L1Lpin D4           //0x01
 #define L1Hpin D5           //0x02
@@ -18,6 +40,13 @@
 #define L3Lpin D9           //0x10
 #define L3Hpin D10          //0x20
 
+PwmOut L1L(L1Lpin);
+PwmOut L1H(L1Hpin);
+PwmOut L2L(L2Lpin);
+PwmOut L2H(L2Hpin);
+PwmOut L3L(L3Lpin);
+PwmOut L3H(L3Hpin);
+
 //Mapping from sequential drive states to motor phase outputs
 /*
 State   L1  L2  L3
@@ -30,92 +59,304 @@
 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
+const int8_t stateMapReversed[] = {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 int8_t lead = 2;  //2 for forwards, -2 for backwards
+
+
+
+//////////////////////
+// Global variables //
+//////////////////////
 
-//Status LED
-DigitalOut led1(LED1);
+volatile float targetSpeed; // User defined instantaneous speed
+volatile float targetPosition;
+volatile float revPerSec = 0; // Instantaneous speed estimate
+volatile float slitPosition = 0;
+volatile float photoPosition = 0;
+volatile float positionEstimate = 0;
+volatile float previousTime = 0; // Used to calculate revPerSec
+volatile float previousSpeed = 0; // Previous value of revPerSec
+volatile float errorPrev = 0.0; //Previous value of error
+volatile float integralError = 0.0; //Integral of the error
+volatile float dutyCycle; // PWM duty cycle between 0 and 1
+volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 }; // Keep track of last state
+Timer timer; // To keep track of time
+Serial pc(SERIAL_TX,SERIAL_RX); // Serial connection
+
 
-//Photointerrupter inputs
-DigitalIn I1(I1pin);
-DigitalIn I2(I2pin);
-DigitalIn I3(I3pin);
+///////////////
+// Functions //
+///////////////
+
+float parseNumber (int position, int *new_pos, char *inPut){
+  char number[8];
+  int count = position;
+  
+  while((inPut[count] > '0' && inPut[count] < '9') || inPut[count] == '-' || inPut[count] == '.')
+  {
+      count ++;   
+  }
+  for (int i = position; i < count; i++)
+  {
+      number[i-position] = inPut[i];
+  }
+      number[count] = '\0';
+      *new_pos = count;
+      return atof(number);  
+  } 
+void parseTones(char *inPut)
+{
+    int count = 1;
+    while (inPut[count] != 13)
+    {
+        //Depends on the format of the tone output
+        }
+    } 
 
-//Motor Drive outputs
-DigitalOut L1L(L1Lpin);
-DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
-DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
-DigitalOut L3H(L3Hpin);
+// THREAD: Parse input
+void parseRegex(char *inPut, float *speed, float *rev){
+    
+        int pos_pointer = 0;
+        switch (inPut[0])
+        {
+            case 'R':
+            //printf("%d\r\n",pos_pointer);
+            *rev = parseNumber(1, &pos_pointer, inPut);
+            //printf("%d\r\n",pos_pointer);
+            if (inPut[pos_pointer] == 'V'){
+                //both R and V defined
+                *speed = parseNumber(pos_pointer+1, &pos_pointer, inPut);
+               // printf("%d\r\n",pos_pointer);
+                }
+            else if (inPut[pos_pointer] != '\r')
+            {
+                 pc.printf("Invalid syntax");
+                }
+            
+            break;
+            case 'V':
+            *speed = parseNumber(1, &pos_pointer, inPut);
+            break;
+            case 'T':
+            parseTones(inPut);
+            break;
+            default:
+            pc.printf("Invalid syntax");
+            //wrong format
+            break;
+            }
+        pc.printf("Revolutions entered: %f, Desired speed: %f\n\r", rev, speed);
+        
+    }
+
+// THREAD: Print instantaneous speed
+void printStatus() {
+    while(1){
+        led3 = !led3;
+        pc.printf("%f\n\r",revPerSec);
+        wait(2);
+    }
+}
 
-//Set a given drive state
-void motorOut(int8_t driveState){
+// THREAD: Control loop
+void controlSpeed(float *targetSpeed){
+    while(true){
+        float error = *targetSpeed - revPerSec;
+        float errorDer = (error - errorPrev);
+        integralError += error;
+        float k = 0.4;
+        float kd = 0.0;
+        float ki = 0.00015;
+        dutyCycle = k*error + ki*integralError;
+        
+        errorPrev = error; //remeber error
+        wait(0.01);
+    }
+}
+
+// THREAD: Control loop
+void controlPosition(float *targetPosition){
+    while(true){
+        positionEstimate = photoPosition + slitPosition;
+        float error = *targetPosition - positionEstimate;
+        float errorDer = (error - errorPrev);
+        float k = 1.0;
+        float kd = 240.0;
+        float limit = 0.5;
+        if (error < 50.0){k = 10.0; limit = 0.2;}
+        dutyCycle = k*error + kd*errorDer;
+        if (dutyCycle < 0) {lead = -2; led1 = 0;} else {lead = 2; led1 = 1;}
+        if (dutyCycle > limit) {dutyCycle = limit;}
+        errorPrev = error; //remeber error
+        wait(0.001);
+    }
+}
+
+// Measure speed using slits
+void measureSpeedSlits(){
+    float currentTime = timer.read();
+    revPerSec = 0.4*previousSpeed + 0.6*((0.0021367521)/(currentTime-previousTime));
+    previousTime = currentTime;
+    previousSpeed = revPerSec;
+}
+
+void measurePositionSlits(){
+    slitPosition += 0.0021367521;
+}
+
+// Measure speed using photointerrupters
+void measureSpeedPhoto(){
+    led3 = !led3;
+    float speedTime;
+    speedTime = timer.read();
+    revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
+    timeMap[I1 + 2*I2 + 4*I3] = speedTime;
+}          
+
+void measurePositionPhoto(){
+    photoPosition += 0.16666666;
+    slitPosition = 0.0;
+}    
     
+// Set motor states
+void motorOut(int8_t driveState, float dutyCycle){
     //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;
+    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 = 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;
-    }
+    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
+//Convert photointerrupter inputs to a rotor state
 inline int8_t readRotorState(){
-    return stateMap[I1 + 2*I2 + 4*I3];
-    }
+    if (lead > 0){return stateMap[I1 + 2*I2 + 4*I3];}
+    else {return stateMapReversed[I1 + 2*I2 + 4*I3];}
+    //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);
-    wait(1.0);
+    motorOut(0,1);
+    wait(7.0);
     
     //Get the rotor state
     return readRotorState();
 }
+
+
+//////////
+// Main //
+//////////     
+
+int main() {
     
-//Main
-int main() {
+    // Initialize threads
+    Thread speedControlThread(osPriorityNormal, DEFAULT_STACK_SIZE/4);
+    Thread positionControlThread(osPriorityNormal, DEFAULT_STACK_SIZE/4);
+    Thread playTones(osPriorityNormal, DEFAULT_STACK_SIZE/4);
+
+    timer.start();
+    
+    //Initialize the serial port
+    Serial pc(SERIAL_TX, SERIAL_RX);
+    pc.printf("Device on \n\r");
+    
     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");
+    
+    float PWM_period = 0.001f; 
+
+    L1L.period(PWM_period);
+    L1H.period(PWM_period);
+    L2L.period(PWM_period);
+    L2H.period(PWM_period);
+    L3L.period(PWM_period);
+    L3H.period(PWM_period);
+    
+    // Slits interrupts
+    CHAintr.rise(&measurePositionSlits);
+    CHBintr.rise(&measurePositionSlits);
+    CHAintr.fall(&measurePositionSlits);
+    CHBintr.fall(&measurePositionSlits);
     
     //Run the motor synchronisation
     orState = motorHome();
-    pc.printf("Rotor origin: %x\n\r",orState);
-    //orState is subtracted from future rotor state inputs to align rotor and motor states
+    
+    // USE FOR PHOTOINTERRUPTERS
+    // Photointerrupters interrupts
+    I1intr.rise(&measurePositionPhoto);
+    I2intr.rise(&measurePositionPhoto);
+    I3intr.rise(&measurePositionPhoto);
+    I1intr.fall(&measurePositionPhoto);
+    I2intr.fall(&measurePositionPhoto);
+    I3intr.fall(&measurePositionPhoto);
     
     //Poll the rotor state and set the motor outputs accordingly to spin the motor
     while (1) {
+        
+        // Read serial input
+        if (pc.readable()){
+            speedControlThread.terminate();
+            positionControlThread.terminate();
+
+            int c = 0;
+            char input[50];
+            char tmp;
+            float rev = 0;
+            float speed = 0;
+            
+            while((tmp = pc.getc()) != '\r'){
+                input[c] = tmp;
+                pc.printf("%c",tmp);
+                c++;
+            }
+            
+            input[c] = '\r';
+            parseRegex(input, &speed, &rev);
+            orState = motorHome();
+            intState = 0;
+            intStateOld = 0;
+            integralError = 0.0;
+            
+            if (rev == 0.0 && speed != 0.0){
+                speedControlThread.start(callback(controlSpeed, &speed));
+            }
+            
+            else if (rev != 0.0 && speed == 0.0){
+                slitPosition = 0.0;
+                photoPosition = 0.0;
+                positionEstimate = 0.0;
+                positionControlThread.start(callback(controlPosition, &rev));
+            }
+        }
+        
         intState = readRotorState();
         if (intState != intStateOld) {
             intStateOld = intState;
-            motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
+            motorOut((intState-orState+2+6)%6, dutyCycle); //+6 to make sure the remainder is positive
         }
     }
-}
-
+}
\ No newline at end of file