Revision 28:4f02ac845e5d, committed 2019-03-16

Comitter:

CallumAlder

Date:

Sat Mar 16 18:16:39 2019 +0000

Parent:

24:be5fef3dace1

Child:

29:c96439a60184

Commit message:

Motor Class

Changed in this revision

--- a/main.cpp	Sat Mar 16 16:43:47 2019 +0000
+++ b/main.cpp	Sat Mar 16 18:16:39 2019 +0000
@@ -8,6 +8,7 @@
 Indx
 newCmd
 MAXCMDLENGTH
+move the global variables to a class because we arent paeasents
 */
 
 //Photointerrupter input pins
@@ -45,6 +46,41 @@
 6       -   -   -
 7       -   -   -
 */
+
+/*//Declare and start threads
+class T_{
+
+    // protected:
+    public:
+
+        Thread *p_comm_in;
+        Thread *p_comm_out;
+        Thread *p_motor_ctrl;
+
+
+
+        T_(){
+            //(priority, stack size,
+            Thread comm_in(osPriorityAboveNormal, 1024);
+            Thread comm_out(osPriorityAboveNormal, 1024);
+            Thread motor_ctrl(osPriorityAboveNormal, 1024);
+
+            p_comm_in = &comm_in;
+            p_comm_out = &comm_out;
+            p_motor_ctrl = &motor_ctrl;
+
+        }
+
+        ~T_(){
+            if (p_comm_in->get_state() == 2)
+                p_comm_in->terminate();
+            if (p_comm_out->get_state() == 2)
+                p_comm_out->terminate();
+            if (p_motor_ctrl->get_state() == 2)
+                p_motor_ctrl->terminate();
+        }
+};*/
+
 //Drive state to output table
 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
 
@@ -55,26 +91,6 @@
 //Phase lead to make motor spin
 const int8_t lead = 2;  //2 for forwards, -2 for backwards
 
-//Status LED
-DigitalOut led1(LED1);
-
-//Photointerrupter inputs
-InterruptIn I1(I1pin);
-InterruptIn I2(I2pin);
-InterruptIn I3(I3pin);
-
-//Motor Drive outputs
-DigitalOut L1L(L1Lpin);
-DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
-DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
-DigitalOut L3H(L3Hpin);
-
-PwmOut pwmCtrl(PWMpin);
-
-uint8_t stateCount[3];   
-uint8_t theStates[3];
 
 class Comm /*: public T_*/{
 
@@ -269,30 +285,9 @@
     void motorCtrlFn() {
         Ticker motorCtrlTicker;
         motorCtrlTicker.attach_us(callback(this,&Comm::motorCtrlTick), 1e5);
-        uint8_t cpyStateCount[3]; 
-        uint8_t cpyCurrentState; 
-        while (_RUN) {
+        while (1) {
             t_motor_ctrl.signal_wait((int32_t)0x1);
-            core_util_critical_section_enter();
-            //Access shared variables here
-            std::copy(stateCount, stateCount+3, cpyStateCount);  
-            // TODO: A thing
-            cpyCurrentState = 0;
-            for (int i = 0; i < 3; ++i) {
-                stateCount[i] = 0; 
-            }
-            core_util_critical_section_exit();
-
-            uint8_t iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount; 
-
-            int16_t totalDegrees = 360 * cpyStateCount[iterElementMax];
-            int16_t stateDiff = theStates[iterElementMax]-cpyCurrentState;
-            if (stateDiff >= 0) {
-                totalDegrees = totalDegrees + (60* stateDiff);  
-            } else {
-                totalDegrees = totalDegrees + (60*stateDiff*-1); 
-            }
-            pc.printf("%u,%u,%u,%u. %.6i \r", iterElementMax, cpyStateCount[0],cpyStateCount[1],cpyStateCount[2], (totalDegrees*10));
+            pc.printf("B4115"); 
         }
     }
 
@@ -311,7 +306,7 @@
 
     Comm() :  pc(SERIAL_TX, SERIAL_RX),
               t_comm_out(osPriorityAboveNormal, 1024),
-              t_motor_ctrl(osPriorityAboveNormal2, 1024)
+              t_motor_ctrl(osPriorityAboveNormal, 1024)
     { // inherit from the RawSerial constructor
 
         pc.printf("%s\n\r", "Welcome" );
@@ -402,64 +397,125 @@
 
 
 
-//Set a given drive state
-void motorOut(int8_t driveState){
+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
+    //Run the motor synchronisation
+    
+    int8_t* params[2];
 
-    //Lookup the output byte from the drive state.
-    int8_t driveOut = driveTable[driveState & 0x07];
+    //Status LED
+    DigitalOut led1(LED1);
+
+    //Photointerrupter inputs
+    InterruptIn I1(I1pin);
+    InterruptIn I2(I2pin);
+    InterruptIn I3(I3pin);
+
+    //Motor Drive outputs
+    DigitalOut L1L(L1Lpin);
+    DigitalOut L1H(L1Hpin);
+    DigitalOut L2L(L2Lpin);
+    DigitalOut L2H(L2Hpin);
+    DigitalOut L3L(L3Lpin);
+    DigitalOut L3H(L3Hpin);
 
-    //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;
+    PwmOut pwmCtrl(PWMpin);
+
+
+
+
+public:
+
+
+
+    float dutyC = 1; // 100%
+    float mtrPeriod = 2e-3; // motor period
+
+
+
+    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;
+        pwmCtrl.pulsewidth(mtrPeriod*dutyC);
+
+        params[0] = &currentState;
+        params[1] = &orState;
 
-    //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;
-}
+        I1.fall(callback(&stateUpdate,params));
+        I2.fall(callback(&stateUpdate,params));
+        I3.fall(callback(&stateUpdate,params));
+
+        I1.rise(callback(&stateUpdate,params));
+        I2.rise(callback(&stateUpdate,params));
+        I3.rise(callback(&stateUpdate,params));
+
+        pwmCtrl.period(mtrPeriod);
+        pwmCtrl.pulsewidth(mtrPeriod*dutyC);
+
+    }
+
+
+        //Set a given drive state
+    void motorOut(int8_t driveState){
+
+        //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;
 
-//Convert photointerrupter inputs to a rotor state
-inline int8_t readRotorState(){
-    return stateMap[I1 + 2*I2 + 4*I3];
-}
+        //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[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(2.0);
+    //Basic synchronisation routine
+    int8_t motorHome() {
+        //Put the motor in drive state 0 and wait for it to stabilise
+        motorOut(0);
+        wait(2.0);
+
+        //Get the rotor state
+        return readRotorState();
+    }
 
-    //Get the rotor state
-    return readRotorState();
+
+    void stateUpdate(int8_t *params[]) { // () { // **params
+        *params[0] = readRotorState();
+        int8_t currentState = *params[0];
+        int8_t offset = *params[1];
+
+        motorOut((currentState - offset + lead + 6) % 6);
+    }
+
 }
 
 
-void stateUpdate(int8_t *params[]) { // () { // **params
-    *params[0] = readRotorState();
-    int8_t currentState = *params[0];
-    int8_t offset = *params[1];
-
-    switch (currentState) {
-        case 1:
-            stateCount[0]++; 
-            break; 
-        case (1 + lead): 
-            stateCount[1]++; 
-            break; 
-        case (1 + (lead*2)):
-            stateCount[2]++; 
-            break; 
-    }
-
-    motorOut((currentState - offset + lead + 6) % 6);
-}
 
 //Main
 int main() {
@@ -470,6 +526,8 @@
     // comm_plz.pc.printf("%s\n", "do i work bruh" ); // using printf of class is calm
     SHA256 Miner;
 
+    Motor motor;
+
     uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
                           0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
                           0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
@@ -485,44 +543,9 @@
     uint32_t hashCounter = 0;
     Timer timer;
 
-    float dutyC = 1; // 100%
-    float mtrPeriod = 2e-3; // motor period
-
-    pwmCtrl.period(mtrPeriod);
-    pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
     comm_plz.start_comm();
 
     // Motor States
-    int8_t orState = 0;    //Rotot offset at motor state 0
-    int8_t currentState = 0;    //Rotot offset at motor state 0
-    int8_t stateList[6];    //Rotot offset at motor state 0
-    //Run the motor synchronisation
-    orState = motorHome();
-
-    theStates[0] = orState;
-    theStates[1] = (orState + lead) % 6;
-    theStates[2] = (orState + (lead*2)) % 6;
-
-    // Add callbacks
-    // I1.fall(&stateUpdate);
-    // I2.fall(&stateUpdate);
-    // I3.fall(&stateUpdate);
-    int8_t* params[2];
-    params[0] = &currentState;
-    params[1] = &orState;
-
-    I1.fall(callback(&stateUpdate,params));
-    I2.fall(callback(&stateUpdate,params));
-    I3.fall(callback(&stateUpdate,params));
-
-    I1.rise(callback(&stateUpdate,params));
-    I2.rise(callback(&stateUpdate,params));
-    I3.rise(callback(&stateUpdate,params));
-
-    // Push motor to move
-    currentState = readRotorState();
-    motorOut((currentState-orState+lead+6)%6); // We push it digitally
 
     // pc.printf("Rotor origin: %x\n\r",orState);
     // orState is subtracted from future rotor state inputs to align rotor and motor states
@@ -533,10 +556,6 @@
     //     motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
     // }
 
-    dutyC = 0.8;
-    pwmCtrl.pulsewidth(mtrPeriod*dutyC);
-
-
     // Keep the program running indefinitely
     timer.start();          // start timer
     int stateCount = 0;