Diff: main.cpp

Revision:

17:80159ace5ddf

Parent:

15:bd303ab8a21f

Child:

18:05e5d280a082

--- a/main.cpp	Tue Mar 20 14:23:18 2018 +0000
+++ b/main.cpp	Tue Mar 20 14:49:51 2018 +0000
@@ -20,6 +20,7 @@
 #define L3Hpin D10          //0x20
 
 #define CHAR_ARR_SIZE 18 //Max length of input codes
+#define MAX_PWM_PERIOD 2000
 
 //Mapping from sequential drive states to motor phase outputs
 /*
@@ -46,9 +47,13 @@
 //Rotor offset at motor state 0
 int8_t orState = 0;
 
+//Set initial torque of 1000
+uint32_t torque = 1000;
+
 
 enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK,
-            MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR,  MSG_TEST};
+            MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR,
+            MSG_TEST, MSG_TORQUE};
 
 //Instantiate the serial port
 RawSerial pc(SERIAL_TX, SERIAL_RX);
@@ -62,11 +67,11 @@
 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);
 
 
@@ -124,25 +129,29 @@
 void setNewCmd(char s[CHAR_ARR_SIZE])
 {
         uint64_t newKey_;
+        uint32_t torque_;
         float  maxspeed_, rotations_pending_;
         //R
         if (sscanf(s, "R%f", &rotations_pending_)) {
-                rotations_pending_mutex.lock();
-                rotations_pending = rotations_pending_;
-                rotations_pending_mutex.unlock();
-                putMessage(MSG_ROT_PEN, rotations_pending);
+            rotations_pending_mutex.lock();
+            rotations_pending = rotations_pending_;
+            rotations_pending_mutex.unlock();
+            putMessage(MSG_ROT_PEN, rotations_pending);
         //V
         } else if (sscanf(s, "V%f", &maxspeed_)) {
-                maxspeed_mutex.lock();
-                maxspeed = maxspeed_;
-                maxspeed_mutex.unlock();
-                putMessage(MSG_MAX_SPD, maxspeed);
+            maxspeed_mutex.lock();
+            maxspeed = maxspeed_;
+            maxspeed_mutex.unlock();
+            putMessage(MSG_MAX_SPD, maxspeed);
         //K
         } else if (sscanf(s, "K%llx", &newKey_)) {
-                newKey_mutex.lock();
-                newKey = newKey_;
-                newKey_mutex.unlock();
-                putMessage(MSG_NEW_KEY, newKey);
+            newKey_mutex.lock();
+            newKey = newKey_;
+            newKey_mutex.unlock();
+            putMessage(MSG_NEW_KEY, newKey);
+        } else if (sscanf(s, "T%u", &torque_)) {
+            torque = torque_;
+            putMessage(MSG_TORQUE, torque);
         //ERROR
         } else
                 putMessage(MSG_INP_ERR, 0x404);
@@ -191,37 +200,37 @@
 
 
 //Set a given drive state
-void motorOut(int8_t driveState){
+void motorOut(int8_t driveState, uint32_t t){
 
     //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_us(0);
     if (~driveOut & 0x02) L1H = 1;
-    if (~driveOut & 0x04) L2L = 0;
+    if (~driveOut & 0x04) L2L.pulsewidth_us(0);
     if (~driveOut & 0x08) L2H = 1;
-    if (~driveOut & 0x10) L3L = 0;
+    if (~driveOut & 0x10) L3L.pulsewidth_us(0);
     if (~driveOut & 0x20) L3H = 1;
 
     //Then turn on
-    if (driveOut & 0x01) L1L = 1;
+    if (driveOut & 0x01) L1L.pulsewidth_us(t);
     if (driveOut & 0x02) L1H = 0;
-    if (driveOut & 0x04) L2L = 1;
+    if (driveOut & 0x04) L2L.pulsewidth_us(t);
     if (driveOut & 0x08) L2H = 0;
-    if (driveOut & 0x10) L3L = 1;
+    if (driveOut & 0x10) L3L.pulsewidth_us(t);
     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);
+    motorOut(0, MAX_PWM_PERIOD);
     wait(2.0);
 
     //Get the rotor state
@@ -231,7 +240,7 @@
 void photointerrupter_isr()
 {
     int8_t intState = readRotorState();
-    motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
+    motorOut((intState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive
 }
 
 
@@ -262,6 +271,8 @@
 
     //Calling the ISR once starts the motor movement
     photointerrupter_isr();
+    
+    
 
     SHA256 sha256;