Diff: main.cpp

Revision:

30:fbae0e5f200d

Parent:

29:c96439a60184

Child:

31:b10ca6cf39bf

--- a/main.cpp	Mon Mar 18 15:37:20 2019 +0000
+++ b/main.cpp	Mon Mar 18 16:24:55 2019 +0000
@@ -81,7 +81,6 @@
 public:
 
     Thread t_comm_out;
-    Thread t_motor_ctrl;
     // Thread *p_motor_ctrl;
 
     bool _RUN;
@@ -154,31 +153,6 @@
 
     }
 
-    /*void commInFn() {
-        // if (_RUN)
-
-        while (_RUN) {
-            osEvent newEvent = inCharQ.get();
-            uint8_t newChar = (uint8_t)(newEvent.value.p); // size_t to type cast the 64bit pointer properly
-            pc.putc(newChar);
-            if(cmdIndx >= MAXCMDLENGTH){            //Make sure there is no overflow in comand.
-                cmdIndx = 0;
-                putMessage(error, 1);
-            }
-            else{
-                if(newChar != '\r'){                //While the command is not over,
-                    newCmd[cmdIndx] = newChar;      //save input character and
-                    cmdIndx++;                      //advance index
-                }
-                else{
-                    newCmd[cmdIndx] = '\0';         //When the command is finally over,
-                    cmdIndx = 0;                    //reset index and
-                    cmdParser();                    //parse the command for decoding.
-                }
-            }
-        }
-    }*/
-
     void returnCursor() {
         pc.putc('>');
         for (int i = 0; i < inCharQIdx; ++i) // reset cursor position
@@ -265,19 +239,7 @@
         }
     }
 
-    // attach_us -> runs funtion every 100ms 
-    void motorCtrlFn() {
-        Ticker motorCtrlTicker;
-        motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5);
-        while (1) {
-            t_motor_ctrl.signal_wait((int32_t)0x1);
-            pc.printf("B4115"); 
-        }
-    }
 
-    void motorCtrlTick(){
-        t_motor_ctrl.signal_set(0x1);
-    }
 
 
     //TODO: stop function, maybe use parent de-constructor
@@ -289,8 +251,7 @@
     Mutex newKey_mutex;         // Restrict access to prevent deadlock.
 
     Comm() :  pc(SERIAL_TX, SERIAL_RX),
-              t_comm_out(osPriorityAboveNormal, 1024),
-              t_motor_ctrl(osPriorityAboveNormal, 1024)
+              t_comm_out(osPriorityAboveNormal, 1024)
     { // inherit from the RawSerial constructor
 
         pc.printf("%s\n\r", "Welcome" );
@@ -370,7 +331,7 @@
         // this::thread::wait()
         // wait(1.0);
         t_comm_out.start(callback(this, &Comm::commOutFn));
-        t_motor_ctrl.start(callback(this, &Comm::motorCtrlFn));
+        
 
 
     }
@@ -381,38 +342,58 @@
 
 
 
-class Motor{
+class Motor {
 
 
 protected:
-    int8_t orState;    //Rotot offset at motor state 0
-    int8_t currentState;    //Rotot offset at motor state 0
-    int8_t stateList[6];    //Rotot offset at motor state 0
+    const int8_t orState;            //Rotor offset at motor state 0, motor specific
+    volatile int8_t currentState;    //Current Rotor State
+    volatile int8_t stateList[6];    //All possible rotor states stored
+
+    //Phase lead to make motor spin
+    int8_t lead;
+
+    Comm* p_comm;
+
     //Run the motor synchronisation
     
-    int8_t* params[2];
+    float dutyC;     // 1 = 100%
+    float mtrPeriod; // motor period
+    uint8_t stateCount[3];  // State Counter
+    uint8_t theStates[3];   // The Key states
 
-    float dutyC; // 1 = 100%
-    float mtrPeriod; // motor period
-    int stateCount;
+    Thread t_motor_ctrl;    // Thread for motor Control
 
 public:
 
-    Motor(){
-
-        orState = motorHome();;    //Rotot offset at motor state 0
-        currentState = readRotorState();;    //Rotot offset at motor state 0
-        stateList[6];    //Rotot offset at motor state 0
-
-        motorOut((currentState-orState+lead+6)%6); // We push it digitally
-
-        dutyC = 0.8;
+    Motor() : t_motor_ctrl(osPriorityAboveNormal, 1024) 
+    {
+        // Set Power to maximum to drive motorHome()
+        dutyC = 1;
         mtrPeriod = 2e-3; // motor period
+        pwmCtrl.period(mtrPeriod);
         pwmCtrl.pulsewidth(mtrPeriod*dutyC);
 
-        params[0] = &currentState;
-        params[1] = &orState;
+        orState = motorHome();             //Rotot offset at motor state 0
+        currentState = readRotorState();   //Current Rotor State
+        // stateList[6] = {0,0,0, 0,0,0};     //All possible rotor states stored
+
+        theStates[0] = orState;
+        theStates[1] = (orState + lead) % 6;
+        theStates[2] = (orState + (lead*2)) % 6;
 
+        lead = 2;  //2 for forwards, -2 for backwards
+       
+        stateCount = 0;
+
+        // p_comm = null; // null pointer for now
+
+    }
+
+
+    void motorStart(Comm &comm) {
+
+        // Establish Photointerrupter Service Routines (auto choose next state)
         I1.fall(callback(this, &Motor::stateUpdate));
         I2.fall(callback(this, &Motor::stateUpdate));
         I3.fall(callback(this, &Motor::stateUpdate));
@@ -420,16 +401,20 @@
         I2.rise(callback(this, &Motor::stateUpdate));
         I3.rise(callback(this, &Motor::stateUpdate));
 
+        // push digitally so if motor is static it will start moving
+        motorOut((currentState-orState+lead+6)%6); // We push it digitally
+
+        // Default a lower duty cylce
+        dutyC = 0.8;
         pwmCtrl.period(mtrPeriod);
         pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-        
-        stateCount = 0;
 
+        // Start motor control thread
+        t_motor_ctrl.start(callback(this, &Comm::motorCtrlFn));
     }
 
-
         //Set a given drive state
-    void motorOut(int8_t driveState){
+    void motorOut(int8_t driveState) {
 
         //Lookup the output byte from the drive state.
         int8_t driveOut = driveTable[driveState & 0x07];
@@ -452,7 +437,7 @@
     }
 
     //Convert photointerrupter inputs to a rotor state
-    inline int8_t readRotorState(){
+    inline int8_t readRotorState() {
         return stateMap[I1 + 2*I2 + 4*I3];
     }
 
@@ -468,25 +453,39 @@
 
 
     void stateUpdate() { // () { // **params
-        *params[0] = readRotorState();
-        int8_t currentState = *params[0];
-        int8_t offset = *params[1];
+        currentState = readRotorState();
 
-        motorOut((currentState - offset + lead + 6) % 6);
-        
-        if (stateCount<6){
-            stateList[stateCount] = currentState;
-            stateCount++;
+        // Store into state counter
+        switch (currentState) {
+        case 1:
+            stateCount[0]++; 
+            break; 
+        case (1 + lead): 
+            stateCount[1]++; 
+            break; 
+        case (1 + (lead*2)):
+            stateCount[2]++; 
+            break; 
         }
-        else {
-            //pc.printf("states");
-            //for(int i = 0; i < 6; ++i)
-            //pc.printf("%02i,", stateList[i]);
-            //pc.printf("\n\r");
-            stateCount = 0;
+        // (Current - Offset + lead + 6) %6
+        motorOut((currentState - orState + lead + 6) % 6);
+
+    }
+
+
+
+    // attach_us -> runs funtion every 100ms 
+    void motorCtrlFn() {
+        Ticker motorCtrlTicker;
+        motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5);
+        while (1) {
+            t_motor_ctrl.signal_wait((int32_t)0x1);
+            pc.printf("B4115"); 
         }
-        
-        
+    }
+
+    void motorCtrlTick(){
+        t_motor_ctrl.signal_set(0x1);
     }
 
 };