Revision 23:904721445e7a, committed 2019-03-20

Comitter:

tanyuzhuo

Date:

Wed Mar 20 17:44:06 2019 +0000

Parent:

21:34f440ae0227

Child:

24:7dd4e187dd30

Commit message:

need position control

Changed in this revision

--- a/main.cpp	Wed Mar 20 12:45:06 2019 +0000
+++ b/main.cpp	Wed Mar 20 17:44:06 2019 +0000
@@ -89,10 +89,11 @@
 uint64_t newKey = 0;
 
 volatile float newRev;
-volatile float maxSpeed = 300;
-uint32_t pulseWidth;
+volatile float maxSpeed = 90;
+float pulseWidth;
 float motorPosition_command;
 float motorPosition;
+//float pulseWidth;
 
 void serialISR() {
     uint8_t newChar = pc.getc();
@@ -310,6 +311,7 @@
                 commandDecoder(command);
             }
             else if (command[0] == 'V') {
+                commandDecoder(command);
                 putMessage(MAX_SPEED_CMD);
             }
             else if (command[0] == 'K') {
@@ -386,7 +388,7 @@
  
  
 //Set a given drive state
-void motorOut(int8_t driveState){
+void motorOut(int8_t driveState, float pulseWidth){
     
     //Lookup the output byte from the drive state.
     int8_t driveOut = driveTable[driveState & 0x07];
@@ -407,9 +409,9 @@
     if (driveOut & 0x10) L3L = 1;
     if (driveOut & 0x20) L3H = 0;
 
-    d9.period(0.002f); //Set PWM period in seconds
-    d9.write(1);     
     
+    //d9.write(1);     
+    d9.pulsewidth_us(pulseWidth);
 }
     
 //Convert photointerrupter inputs to a rotor state
@@ -419,7 +421,7 @@
 
 int8_t motorHome() {
     //Put the motor in drive state 0 and wait for it to stabilize
-    motorOut(0);
+    motorOut(0,pulseWidth);
     wait(2.0);
     return readRotorState();
 }
@@ -431,7 +433,7 @@
     //orState is subtracted from future rotor state inputs to align rotor and motor states   
     
     int8_t rotorState = readRotorState();
-    motorOut((rotorState-orState+lead+6)%6); //+6 to make sure the remainder is positive
+    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);
@@ -451,10 +453,10 @@
 void motorCtrlFn(){
     int32_t counter=0;
     static int32_t oldmotorPosition;
-    uint32_t Ts;
+    float  Ts;
     
     float Speed;
-    float kps = 25; //proportional constant for speed
+    float kps = 40; //proportional constant for speed
     // Timer to count time passed between ticks to calculate velocity
     Timer motorTime;
     motorTime.start();
@@ -479,7 +481,13 @@
         
         //equation for controls
         Ts = kps*(Speed -abs(motorVelocity));//*errorSign;
-      
+        if (Ts > 2000){
+            Ts = 2000;    
+        }
+        else if (Ts < 0){
+            Ts = 0;   
+        }
+        pulseWidth = Ts;
        // Mp = ks*error + kd*(error - PrevError) /motorTime.read() + ki*errorSum;
  
         motorTime.reset();
@@ -503,9 +511,8 @@
     
     
    
-    //PWM.period(0.002f); //Set PWM period in seconds
-    //PWM.write(0.5);     //Set PWM duty in %
     
+    d9.period(0.002f); //Set PWM period in seconds
 
 
     commandProcessorthread.start(commandProcessor);