Revision 24:7dd4e187dd30, committed 2019-03-20

Comitter:

tanyuzhuo

Date:

Wed Mar 20 23:46:05 2019 +0000

Parent:

23:904721445e7a

Child:

25:ca5ccbf55b07

Commit message:

fuck EEE

Changed in this revision

--- a/main.cpp	Wed Mar 20 17:44:06 2019 +0000
+++ b/main.cpp	Wed Mar 20 23:46:05 2019 +0000
@@ -44,7 +44,7 @@
 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
 
 //Phase lead to make motor spin
-const int8_t lead = -2;  //2 for forwards, -2 for backwards
+int8_t lead = -2;  //2 for forwards, -2 for backwards
 
 //Status LED
 DigitalOut led1(LED1);
@@ -144,18 +144,11 @@
                     pc.printf("Not a valid key!\n\r");
                     break;
                 case ROTATION_CMD :
-                    pc.printf("Rotation command\n\r"); 
+                    pc.printf("Rotation count: %3.2f\n\r", mail->fdata);
                     break; 
                 case MAX_SPEED_CMD :
                     pc.printf("Max speed command\n\r"); 
                     break; 
-                case R_ECHO1 :
-                    pc.printf("Rotor command:\n\r");
-                    pc.printf("Full rotations: %d\n\r", mail->data); 
-                    break; 
-                case R_ECHO2 :
-                    pc.printf("Partial rotation: %d\n\r", mail->data); 
-                    break; 
                 case MAX_SPEED_ECHO1 :
                     pc.printf("Max speed command:\n\r");
                     pc.printf("Max speed int: %d\n\r", mail->data); 
@@ -241,10 +234,11 @@
                     decimalValue = (decimalValue/10 + command[index] - '0')/10;
                 }
             }
-            putMessage(R_ECHO1, intValue);
-            putMessage(R_ECHO2, (int) (100*decimalValue));
             decimalValue = (decimalValue + intValue) * sign;
-            //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH    
+            putMessage(ROTATION_CMD,0,decimalValue);
+            //HERE SEND IT TO ANY GLOBAL VARIABLE YOU WISH 
+            
+            newRev = decimalValue;
             break; 
             
         case 'V' :
@@ -307,8 +301,11 @@
             command[17] = '\0';
             
             if (command[0] == 'R') {
+                commandDecoder(command);
+                motorPosition_command = motorPosition;
                 putMessage(ROTATION_CMD);
-                commandDecoder(command);
+                
+                
             }
             else if (command[0] == 'V') {
                 commandDecoder(command);
@@ -453,43 +450,116 @@
 void motorCtrlFn(){
     int32_t counter=0;
     static int32_t oldmotorPosition;
-    float  Ts;
-    
+    float Ts;
+    float sError;      //speed error
+    float intError = 0; //integral of velError
+    float Tr;
+    float error;
+    float olderror = 0;
     float Speed;
-    float kps = 40; //proportional constant for speed
+    float Rev;
+    int8_t leads = -2;
+    int8_t leadr = -2;
+    float kps = 80;
+    float kis = 0.07;
+    float kpr = 35;
+    float kdr = 14.75; //proportional constant for speed
     // Timer to count time passed between ticks to calculate velocity
     Timer motorTime;
     motorTime.start();
     float motorPos;
-    //float ki = ??; // integration constant, to be tested for friction
+    float myTime;
+    
     Ticker motorCtrlTicker;
     motorCtrlTicker.attach_us(&motorCtrlTick,100000);
     while(1){
         motorCtrlT.signal_wait(0x1);
-//        errorSum= 0;
-//        for(uint8_t i=9; i >0 ; i--){
- //           PrevErrorArray[i] = prevErrorArray[i-1];
- //           errorSum+= PrevErrorArray[i];
-               
-        // convert state change into rotations
+
         Speed = maxSpeed*6;
-        
+        Rev = newRev *6;
         motorPos = motorPosition;
-        //pc.printf ("motor Pos: %f\n\r", motorPos);
-        motorVelocity = (motorPos - oldmotorPosition)/motorTime.read();
+        
+        myTime = motorTime.read();
+        motorVelocity = (motorPos - oldmotorPosition)/myTime;
+        error = Rev + motorPosition_command - motorPos; 
         oldmotorPosition = motorPos;
         
         //equation for controls
-        Ts = kps*(Speed -abs(motorVelocity));//*errorSign;
+        sError = Speed -abs(motorVelocity);
+        intError = intError + sError;
+        Ts = (kps * sError + kis * intError)*(error-olderror/abs(error-olderror));
+        Tr = kpr*error+kdr*(error-olderror)/ myTime;
+        //pc.printf("Error: %f\n\r", Tr);
+        
+        // Lead flip
+        if ( Ts >0){
+            leads = 2;
+        }
+        else if ( Ts <0){
+            leads = -2;
+        }  
+       
+        if (Tr >= 0){
+            leadr= -2;    
+        }
+        else if (Tr < 0){
+            leadr = 2;   
+        }
+        
+        //Torque adjustment
         if (Ts > 2000){
+            
             Ts = 2000;    
         }
         else if (Ts < 0){
-            Ts = 0;   
+           
+            Ts = -Ts;    
+        }
+        else if (Ts < -2000){
+            
+            Ts = 2000;    
+        }
+        if (Tr > 2000){
+            
+            Tr = 2000;    
+        }
+        else if (Tr < 0){
+           
+            Tr = -Tr;   
+        }
+        else if (Tr < -2000){
+           
+            Tr = 2000;    
         }
-        pulseWidth = Ts;
-       // Mp = ks*error + kd*(error - PrevError) /motorTime.read() + ki*errorSum;
- 
+        ;
+        //if (motorVelocity < 0){
+           if (Ts <=Tr ){
+             lead = leads;
+             pulseWidth = Ts;
+            
+           }
+           else{
+             lead = leadr;
+             pulseWidth = Tr;
+           
+           } /*
+        }
+        else{
+            if (Ts <=Tr ){
+            lead = leads; 
+            pulseWidth = Ts;
+             
+            }
+            else{
+            lead = leadr;
+            pulseWidth = Tr;
+            
+            } 
+        }*/
+            
+        
+        
+       
         motorTime.reset();
         // Serial output to monitor speed and position
         counter++;
@@ -501,6 +571,7 @@
             putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
             //pc.printf ("torque: %f\n\r", Ts);
         }
+        olderror=error;
     }
 }
 int main() {