Diff: main.cpp

Revision:

18:e48c0910c71e

Parent:

17:ff5300ba5442

Child:

20:5bd9f9e406d1

--- a/main.cpp	Sat Mar 16 15:53:32 2019 +0000
+++ b/main.cpp	Mon Mar 18 16:30:12 2019 +0000
@@ -55,21 +55,25 @@
 InterruptIn I3(I3pin);
 
 //Motor Drive outputs
-DigitalOut L1L(L1Lpin);
+PwmOut L1L(L1Lpin);
 DigitalOut L1H(L1Hpin);
-DigitalOut L2L(L2Lpin);
+PwmOut L2L(L2Lpin);
 DigitalOut L2H(L2Hpin);
-DigitalOut L3L(L3Lpin);
+PwmOut L3L(L3Lpin);
 DigitalOut L3H(L3Hpin);
-PwmOut PWM(PWMpin);
+
 
 int8_t orState = 0;
 int8_t intState = 0;
 int8_t intStateOld = 0;
 int32_t revoCounter = 0;    //Counts the number of revolutions
+int32_t motorVelocity;
+//Phase lead to make motor spin
+int8_t lead = -2;  //2 for forwards, -2 for backwards
 
 typedef struct {
   uint64_t nonce;
+  float data;
 } mail_t;
  
 Mail<mail_t, 16> mail_box;
@@ -80,9 +84,17 @@
 Mutex newKey_mutex;
 uint64_t newKey = 0;
 
-void putMessage(uint64_t *nonce){
+volatile float newRev;
+volatile float maxSpeed = 300;
+uint32_t pulseWidth;
+float motorPosition_command;
+float motorPosition;
+
+// mail to queue messages for serial port
+void putMessage(uint64_t *nonce,float data){
     mail_t *mail = mail_box.alloc();
     mail->nonce = *nonce;
+    mail->data = *data;
     mail_box.put(mail);
 }
 
@@ -108,6 +120,7 @@
             
             if (command[0] == 'R') {
                 pc.printf("Rotation command\n");                
+                
                 pc.printf("%s", command);
             }
             else if (command[0] == 'V') {
@@ -180,26 +193,34 @@
         t.stop();
     }
     }
+    
+    
+    
+void motorCtrlTick(){
+ motorCtrlT.signal_set(0x1);
+ }
+ 
+ 
 //Set a given drive state
-void motorOut(int8_t driveState){
+void motorOut(int8_t driveState,uint32_t motorTorque){
     
     //Lookup the output byte from the drive state.
     int8_t driveOut = driveTable[driveState & 0x07];
       
     //Turn off first
-    if (~driveOut & 0x01) L1L = 0;
+    if (~driveOut & 0x01) L1L.pulsewidth(0);
     if (~driveOut & 0x02) L1H = 1;
-    if (~driveOut & 0x04) L2L = 0;
+    if (~driveOut & 0x04) L2L.pulsewidth(0);
     if (~driveOut & 0x08) L2H = 1;
-    if (~driveOut & 0x10) L3L = 0;
+    if (~driveOut & 0x10) L3L.pulsewidth(0);
     if (~driveOut & 0x20) L3H = 1;
     
     //Then turn on
-    if (driveOut & 0x01) L1L = 1;
+    if (driveOut & 0x01) L1L.pulsewidth(motorTorque);
     if (driveOut & 0x02) L1H = 0;
-    if (driveOut & 0x04) L2L = 1;
+    if (driveOut & 0x04) L2L.pulsewidth(motorTorque);
     if (driveOut & 0x08) L2H = 0;
-    if (driveOut & 0x10) L3L = 1;
+    if (driveOut & 0x10) L3L.pulsewidth(motorTorque);
     if (driveOut & 0x20) L3H = 0;
 }
     
@@ -209,19 +230,65 @@
 }
 
 int8_t motorHome() {
-    motorOut(0);
+    //Put the motor in drive state 0 and wait for it to stabilize
+    L1L.period(2000);
+    L2L.period(2000);
+    L3L.period(2000);
+    motorOut(0,200);
     wait(2.0);
     return readRotorState();
 }
 
-void push() {
+//orState is subtracted from future rotor state inputs to align rotor and motor states   
+int8_t orState = motorHome();
+// ISR to handle the updating of the motor position
+void motorISR() {
+    static int8_t oldRotorState;
+    int8_t rotorState = readRotorState();
+    motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive
+    if (rotorState - oldRotorState == 5) motorPosition --;
+    else if (rotorState - oldRotorState == -5) motorPosition ++;
+    else motorPosition += (rotorState - oldRotorState);
+    oldRotorState = rotorState;
+}
+/*void push() {
     intState = readRotorState();
     if (intState != intStateOld) {
         intStateOld = intState;
         motorOut((intState - orState + lead +6) % 6); //+6 to make sure the remainder is positive
     }
-}
+}*/
 
+void motorCtrlFn(){
+    int32_t counter=0;
+    static int32_t oldmotorPosition;
+    // Timer to count time passed between ticks to calculate velocity
+    Timer motorTime;
+    motorTime.start();
+    float motorPos;
+    float windingSpeed;
+    float windingRev;
+    
+    Ticker motorCtrlTicker;
+    motorCtrlTicker.attach_us(&motorCtrlTick,100000);
+    while(1){
+        motorCtrlT.signal_wait(0x1);
+        // convert state change into rotations
+        windingSpeed = maxSpeed*6;
+        windingRev = newRev*6;
+        motorPos = motorPosition;
+        motorVelocity = (motorPos - oldmotorPosition)/motorTime.read();
+        oldmotorPosition = motorPos;
+        motorTime.reset();
+        // Serial output to monitor speed and position
+        counter++;
+        if(counter == 10){
+            counter = 0;
+            //display velocity and motor position 
+            putMessage(3,(float)(motorPos/6.0));
+            putMessage(4,(float)(motorVelocity/6.0));
+        }
+    }
 int main() {    
     //Serial pc(SERIAL_TX, SERIAL_RX);
     
@@ -231,8 +298,8 @@
     commandProcessorthread.start(commandProcessor);
     bitcointhread.start(bitcoin);
     
-    PWM.period(0.002f); //Set PWM period in seconds
-    PWM.write(0.5);     //Set PWM duty in %
+    //PWM.period(0.002f); //Set PWM period in seconds
+    //PWM.write(0.5);     //Set PWM duty in %
 
     pc.printf("Hello Pete\n\r");