Diff: main.cpp

Revision:

3:659998b2bee1

Parent:

2:93918cad51dd

Child:

4:6dec99ee29e6

--- a/main.cpp	Wed Oct 17 12:23:50 2018 +0000
+++ b/main.cpp	Thu Oct 18 14:16:49 2018 +0000
@@ -13,44 +13,70 @@
 InterruptIn motor1B(D12);   // Encoder 1b
 QEI         Encoder1(D12,D13,NC,64);    // Note that we use X2 encoding, so there are 4200 counts/rotation, as opposed to 8400 from the documentation.
 
-DigitalOut directionpin(D7);
-PwmOut pwmpin(D6);
+DigitalOut directionpin1(D7);
+PwmOut pwmpin1(D6);
 Serial pc(USBTX, USBRX); // tx, rx
 
-const int CpR = 4200; //When the count is 4200, 1 revolution is made. (Using X2 encoding)
-const int MAlength = 100; // Set the length of the MA
-int MovingAverageArray[MAlength] = {pot1*CpR}; // Make an array where we can save 100 values to get an average value from our potentiometer.
-int count = 0; // Counter for the moving average array
+Ticker Motor1;
 
-void SetMotor(){
-        int counts1 = -1*Encoder1.getPulses(); // Get counts from encoder 1 (motor 1). Notice the minus sign; CCW movement = positive counts.
-        int DesiredCounts = pot1*CpR; // The potmeter sets the desired amount of counts.
-            
-        scope.set(0,counts1);
-        scope.set(1,DesiredCounts);
-        scope.send();
-        
-        // we need the following: if amount of counts by the encoder =/= desiredcounts, turn on the motor until it is equal. if aoc = dc, do nothing.
-        if(counts1 != DesiredCounts){
-            // Check which direction we have to move; when DC > AOC, we have to move CW and vice versa.
-            if(DesiredCounts>counts1){
-                directionpin=1; // still need to check whether directionpin = 1 makes the motor rotate CW.
-                }
-                else{
-                    directionpin=0;
-                }
-                pwmpin = 1;
-                pc.printf("Counts from potentiometer: %d. Average of 100 (counts): %d \n", count, AvgCounts);
-                }
-                else{
-                    pwmpin = 0; // when aoc = dc, the motor does not rotate.
-                    }
+double GetReferencePosition(){
+    // Returns the wanted reference position
+    const int CpR = 4200; //Counts Per Revolution; when the counts i 4200, 1 revolution has been made. (Using X2 encoding)
+    double ReferencePosition = CpR*pot1; // Returns a value between 0 and 4200, the wanted rotation. 
+    return ReferencePosition;
+}
+    
+double GetActualPosition(){
+    double ActualPosition = -Encoder1.getPulses(); // Gets the actual amount of counts from motor 1. Note the minus sign for convention (CCW = positive counts.)
+    return ActualPosition;
+}
+    
+double P_controller(double error){
+    double Kp = pot2*17+1;
+    double u_k = Kp*error;  // Proportional part
+    
+    // Sum all parts and return it
+    return u_k;
 }
 
-int main() {
-    pwmpin.period_us(60);   // Set pwm period
-    while (true) {
-        SetMotor();
-        wait(0.001f);
-    }
-}
\ No newline at end of file
+void SetMotor1(double motorValue){
+    // Given -1<=motorValue<=1, this sets the PWM and direction
+    // bits for motor 1. Positive value makes motor rotating
+    // clockwise. motorValues outside range are truncated to
+    // within range
+    if (motorValue >=0) directionpin1=1;
+        else directionpin1=0;
+    if (fabs(motorValue)>1) pwmpin1 = 1;
+        else pwmpin1 = fabs(motorValue);
+}
+
+void ScopeData(){
+        scope.set(0,pot1*4200);              // Wanted amount of counts
+        scope.set(1,-Encoder1.getPulses());  // Amount of counts of motor 1
+        scope.set(2,pwmpin1);                // Current pwm-value send to motor 1
+        scope.send(); // send what's in scope memory to PC
+}
+
+void MeasureAndControl(void){
+    // This function determines the desired velocity, measures the
+    // actual velocity, and controls the motor with 
+    // a simple Feedback controller. Call this from a Ticker.
+    float ReferencePosition = GetReferencePosition();
+    float ActualPosition = GetActualPosition();
+    float motorValue = P_controller(ReferencePosition - ActualPosition);
+    SetMotor1(motorValue);
+    ScopeData();
+}
+
+int main(){
+    pwmpin1.period_us(60);
+  //  Motor1.attach(&MeasureAndControl, 0.01); 
+    while(1){
+            float ReferencePosition = GetReferencePosition();
+            float ActualPosition = GetActualPosition();
+            float motorValue = P_controller(ReferencePosition - ActualPosition);
+            SetMotor1(motorValue);
+            ScopeData();
+            wait(0.01f);
+        }
+    }
\ No newline at end of file