Diff: main.cpp

Revision:

7:08fd3bc7a3cf

Parent:

6:ea3b3f50c893

Child:

11:ca91fc47ad02

--- a/main.cpp	Fri Oct 11 12:42:36 2019 +0000
+++ b/main.cpp	Fri Oct 11 13:40:36 2019 +0000
@@ -7,30 +7,23 @@
 
 DigitalIn button1(D12);
 AnalogIn pot2(A0);
+AnalogIn pot1(A1);
 Ticker motor; //ticker to call motor values
 DigitalOut motor1Direction(D7); // is a boolean
-FastPWM motor1Velocity(D6);
+FastPWM motor1absolutemotorvalueocity(D6);
 MODSERIAL pc(USBTX, USBRX);
 QEI Encoder(D8,D9,NC,8400);
+
+
 volatile double frequency = 17000.0;
 volatile double period_signal = 1.0/frequency;
-float vel = 0.0f;
-float referencevelocity;
-float motorvalue2;
+float timeinterval = 0.001f;
 int counts;
-float position1 = 0;
-float position2;
-float position11;
-float position22;
-float timeinterval = 0.001f;
-float measuredvelocity;
-double Kp = 5;
-double Ki = 0.1;
-double Kd = 1;
+float measuredposition;
 float motorvalue;
-float desiredposition; 
-float measuredposition;
-float referenceposition;
+double u_i;
+float potmeter1 = pot1.read();
+
 
 
 
@@ -44,6 +37,9 @@
     static double error_integral = 0;
     static double error_prev = error;
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    double Kp = pot1.read()*10;
+    double Ki = 0.1;
+    double Kd = 0.1;
     
     //Proportional part:
     double u_k = Kp * error;
@@ -51,6 +47,11 @@
     // Integreal part
     error_integral = error_integral + error * timeinterval;
     double u_i = Ki*error_integral;
+    // anti wind up
+    if (u_i > 10)
+    {
+        u_i = 10 ;   
+    }
     
     // Derivate part
     double error_derivative = (error - error_prev)/timeinterval;
@@ -66,31 +67,31 @@
 double getmeasuredposition()
 {   
     counts = Encoder.getPulses();
-    counts = counts % 8400;
     measuredposition = ((float)counts) / 8400.0f * 2.0f;
     
     return measuredposition;
 }
 
-//get the reference of the velocity
+//get the reference of the absolutemotorvalueocity
 double getreferenceposition()
 {
-    referenceposition = 1; //this determines the amount of spins
+    float referenceposition;
+    referenceposition = -1+2*pot2.read(); //this determines the amount of spins
     return referenceposition;
 }   
 
 //send value to motor
 void sendtomotor(float motorvalue)
 {   
-    motorvalue2 = motorvalue;
-    vel = fabs(motorvalue);
-    vel = vel > 1.0f ? 1.0f : vel;   //if velocity is greater than 1, reduce to 1, otherwise remain vel
-    motor1Velocity = vel;
+    float absolutemotorvalue = 0.0f;
+    absolutemotorvalue = fabs(motorvalue);
+    absolutemotorvalue = absolutemotorvalue > 1.0f ? 1.0f : absolutemotorvalue;   //if absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
+    motor1absolutemotorvalueocity = absolutemotorvalue;
 
     motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
 }
 
-// function to call reference velocity, measured velocity and controls motor with feedback
+// function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
 void measureandcontrol(void)
 {
     float referenceposition = getreferenceposition();
@@ -102,28 +103,26 @@
 {
     pc.baud(115200);
     pc.printf("Starting...\r\n");
-    motor1Velocity.period(period_signal);
+    motor1absolutemotorvalueocity.period(period_signal);
     motor.attach(measureandcontrol, timeinterval); 
     while(true)
     {
         wait(0.5);
-        //pc.printf("velocity = %f\r\n", vel);
-        //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
-        //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
         pc.printf("number of counts %i\r\n", counts);
         pc.printf("measured position is %f\r\n", measuredposition);
-        //pc.printf("position1 = %f\r\n",position11);
-        //pc.printf("position2 = %f\r\n",position22);
         pc.printf("motorvalue is %f\r\n", motorvalue);
+        pc.printf("u_i is %d\r\n", u_i);
+        pc.printf("potmeter 1 gives %f\r\n", potmeter1);
         
         char c = pc.getcNb();
         
+        // type c to stop the program
         if (c == 'c')
         {
-            pc.printf("Programm stops running");
+            pc.printf("Program stopped running");
             motorvalue = 0;
             sendtomotor(motorvalue);
-            return -1;
+            break;
         }
     }
 }
\ No newline at end of file