Revision 21:34f440ae0227, committed 2019-03-20

Comitter:

tanyuzhuo

Date:

Wed Mar 20 12:45:06 2019 +0000

Parent:

20:5bd9f9e406d1

Child:

22:de980b82d0ce

Child:

23:904721445e7a

Commit message:

fixed display error, need commin decoding correction

Changed in this revision

--- a/main.cpp	Tue Mar 19 22:53:22 2019 +0000
+++ b/main.cpp	Wed Mar 20 12:45:06 2019 +0000
@@ -278,7 +278,7 @@
             putMessage(MAX_SPEED_ECHO2, (int) (100*decimalValue));
             decimalValue = (decimalValue + intValue);
             //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH
-           // maxSpeed = decimalValue;
+            maxSpeed = decimalValue;
             break;
              
         case 'K' :
@@ -407,6 +407,8 @@
     if (driveOut & 0x10) L3L = 1;
     if (driveOut & 0x20) L3H = 0;
 
+    d9.period(0.002f); //Set PWM period in seconds
+    d9.write(1);     
     
 }
     
@@ -449,10 +451,10 @@
 void motorCtrlFn(){
     int32_t counter=0;
     static int32_t oldmotorPosition;
-   // uint32_t Ts;
+    uint32_t Ts;
     
-  //  float Speed;
-  //  float kps = 25; //proportional constant for speed
+    float Speed;
+    float kps = 25; //proportional constant for speed
     // Timer to count time passed between ticks to calculate velocity
     Timer motorTime;
     motorTime.start();
@@ -466,9 +468,9 @@
 //        for(uint8_t i=9; i >0 ; i--){
  //           PrevErrorArray[i] = prevErrorArray[i-1];
  //           errorSum+= PrevErrorArray[i];
-            }    
+               
         // convert state change into rotations
-        //Speed = maxSpeed*6;
+        Speed = maxSpeed*6;
         
         motorPos = motorPosition;
         //pc.printf ("motor Pos: %f\n\r", motorPos);
@@ -476,8 +478,8 @@
         oldmotorPosition = motorPos;
         
         //equation for controls
-        //Ts = kps*(Speed -abs(motorVelocity));//*errorSign;
-        //pc.printf ("torque: %f\n\r", Ts);
+        Ts = kps*(Speed -abs(motorVelocity));//*errorSign;
+      
        // Mp = ks*error + kd*(error - PrevError) /motorTime.read() + ki*errorSum;
  
         motorTime.reset();
@@ -487,19 +489,18 @@
             counter = 0;
             //display velocity and motor position 
            // pc.printf ("motor Pos: %f\n\r", (motorPosition/6.0));
-            putMessage(MOTOR_POS,0,(float)(motorPos/6.0));
+            putMessage(MOTOR_POS,0,(float)(motorPosition/6.0));
             putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
+            //pc.printf ("torque: %f\n\r", Ts);
         }
     }
-
+}
 int main() {    
     //Serial pc(SERIAL_TX, SERIAL_RX);
     
     //Initialise bincoin mining and communication
     
     
-    d9.period(0.002f); //Set PWM period in seconds
-    d9.write(1);     
     
    
     //PWM.period(0.002f); //Set PWM period in seconds