Diff: main.cpp

Revision:

3:91e4ed7b6748

Parent:

2:68de33d10c67

Child:

4:840ce3b21767

--- a/main.cpp	Wed Oct 11 09:49:09 2017 +0000
+++ b/main.cpp	Fri Oct 20 10:20:59 2017 +0000
@@ -3,37 +3,67 @@
 #include "math.h"
 #include "encoder.h"
 
-DigitalOut gpo(D0);
 DigitalOut motorDirection(D4);
 PwmOut motorSpeed(D5);
 AnalogIn potMeterIn1(A1);
 AnalogIn potMeterIn2(A2);
-//InterruptIn button1(D3);
+InterruptIn button(D11);
 Ticker m1_Ticker;
+Ticker SP_m1_Ticker;
 Encoder encoder1(D13,D12);
-const double M1_KP = 2.5, M1_KI = 1.0;
-const double M1_TS = 0.01;
-const double RAD_PER_PULSE = 0.002991;
-double m1_err_int = 0;
+float M1_KP = 2.5;
+float M1_KI = 1.0;
+const float M1_TS = 0.01;
+const float SP_TS = 0.01;
+const float RAD_PER_PULSE = 0.000119;
+float m1_err_int = 0;
 int motorD = 0;
-double motor1 = 0;
+float motor1 = 0;
+float reference = 0;
+float position = 0;
+int outOfEncod = 0;
+int KP_changer = 1;
+float RotSpeed = 0;
+
+int outOfEncod_SP_1 = 0;
+int outOfEncod_SP_2 = 0;
+
 
 MODSERIAL pc(USBTX, USBRX);
 
 
                                                                                     // Reusable PI controller
-double PI( double e, const double Kp, const double Ki, double Ts, double &e_int ){
-    e_int += Ts ∗ e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
-    return Kp ∗ e + Ki ∗ e_int;
+float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ){
+    e_int += Ts * e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
+    return Kp * e + Ki * e_int;
+}
+
+void Change()
+{
+    if(KP_changer == 60){
+    KP_changer = 0;
+    } 
+    else{
+    KP_changer++;
+    }
+}
+
+void m1_Velocity(){
+    outOfEncod_SP_1 = encoder1.getPosition();
+    float RotDiff = (outOfEncod_SP_1 - outOfEncod_SP_2)*RAD_PER_PULSE;
+    outOfEncod_SP_2 = outOfEncod_SP_1;
+    RotSpeed = RotDiff/SP_TS;
 }
                                                                                     // Next task, measure the error and apply the output to the plant
 void m1_Controller() {
-    double reference = potMeterIn1;
-    
-    
-    double position = RAD_PER_PULSE∗encoder1;                         // Don’t use magic numbers!
-    motor1 = PI( reference − position, M1_KP, M1_KI, M1_TS, m1_err_int );
-    if ( reference − position >= 0) 
+    reference = 5 * potMeterIn1.read();
+    M1_KI = 2 * potMeterIn2.read();
+    M1_KP = KP_changer*0.1;
+    outOfEncod = encoder1.getPosition();
+    position = RAD_PER_PULSE * outOfEncod;
+    float ref_pos = reference - position;                                          // Don’t use magic numbers!
+    motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int );
+    if (ref_pos <= 0) 
         {
             //float motor1DirectionPin1 = 1;
             motorD=1;
@@ -43,14 +73,25 @@
             //float motor1DirectionPin1 = 0;
             motorD=0;
         }
+        motorDirection.write(motorD);
+        motorSpeed.write(motor1);
 }
 
 int main() {
-    m1_Ticker.attach( &m1_Controller, M1_TS );                                      // 100 Hz
-    while( 1 ) {
-        motorDirection.write(motorD);
-        motorSpeed.write(motor1);        
+    m1_Ticker.attach( &m1_Controller, M1_TS );    
+    SP_m1_Ticker.attach( &m1_Velocity, SP_TS );                                    // 100 Hz
+    pc.baud(115200);
+    pc.printf("Fuck you pc");
+    m1_Controller();
+    button.rise(&Change);
+    while(true) {
+        pc.printf("%f \t", RotSpeed);
+        pc.printf("%f \t", M1_KI);      
+        pc.printf("%f \t", M1_KP);      
+        pc.printf("%f \t", position); 
+        pc.printf("%f \r\n", reference);
+        //wait(0.5);    
     }
 }
-}
+
     
\ No newline at end of file