Diff: main.cpp

Revision:

1:83fccd7d8483

Parent:

0:46582bba07d2

Child:

2:57523bb4e9c6

--- a/main.cpp	Mon Oct 17 13:53:44 2016 +0000
+++ b/main.cpp	Mon Oct 24 10:54:55 2016 +0000
@@ -1,79 +1,174 @@
 #include "mbed.h"
 #include "QEI.h"
+#define pi 3.14159265359;
 
 PwmOut Motor1_pwm(D5);
-QEI Encoder(D12,D13,NC,64);
+DigitalOut SlideMotor_Direction(D4);
+PwmOut Motor2_pwm(D6);
+DigitalOut LiftMotor_Direction(D7);
+AnalogIn Potmeter(A0);
+QEI Slide_Encoder(D12,D13,NC,64);
+QEI Lift_Encoder(D10,D11,NC,64);
 Serial pc(USBTX,USBRX);
 Ticker Controller;
 
-float Radius = 10;
+char Key;
+bool Controller_Flag=0;
 float Frequency = 30;
-float Multiplier = 1;
-float k1 = 1;
-float k2 = 0.1f;
-float k3 = 0.1f;
-float Start_slow = 100;
-float Start_lock = 60;
-float End_slow = 400;
-float End_lock = 440;
 float Frequency_PWM = 10000;
-float Counts = 0;
-float Revolutions = 0;
-const double pi = 3.14159265359;
+
+float Slide_Radius = 12.5;
+float Slide_Multiplier = 1;
+float k1 = 1;
+float k2 = 0.02f;
+float k3 = 0.1f;
+float Start_slow = 40;
+float Start_lock = 0;
+float End_slow = 340;
+float End_lock = 380;
+float Slide_Counts;
+float Slide_Revolutions;
+float Slide_Angle;
+float Slide_Position;
 
-float Input_force = 0.1;
-float Angle = 0;
-float Curr_speed = 0;
-float Desired_speed;
-float Delta_speed;
-float Int_delta_speed;
-float Deriv_delta_speed = 0;
-float Prev_delta_speed = 0;
+float Slide_Input_force = 0;
+float Slide_Curr_speed = 0;
+float Slide_Desired_speed;
+float Slide_Delta_speed;
+float Slide_Int_delta_speed;
+float Slide_Deriv_delta_speed = 0;
+float Slide_Prev_delta_speed = 0;
+float Slide_PI;
+
+float Lift_Radius = 10;
+float Lift_Multiplier = 1;
+float Lift_k1 = 0.08;
+float Lift_k2 = 0.05;
+float Lift_k3 = 0.01;
+float Lift_Start = 0;
+float Lift_End = 50;
+float Lift_Counts;
+float Lift_Revolutions;
+float Lift_Angle;
+float Lift_Position;
+
+float Lift_Input_force = 0;
+float Lift_Desired_position;
+float Lift_Delta_position;
+float Lift_Int_delta_position;
+float Lift_Deriv_delta_position = 0;
+float Lift_Prev_delta_position = 0;
+float Lift_PI;
 
 void Slide_Controller();
+void Lift_Controller();
+void Ticker_Flag();
 
 int main()
 {
     Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
-   
-    Controller.attach(&Slide_Controller,1/Frequency);
+    Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
+    Controller.attach(&Ticker_Flag,1/Frequency);
     pc.baud(9600);
     while (true) {
-        Counts = Encoder.getPulses();
-        Revolutions = Counts /(32*131);
-        Angle = Revolutions*2*pi;
-        char a = pc.getc();
-        Input_force = (float)a/100;
-            
-        pc.printf("%f", Input_force);
+        
+        Slide_Counts =      Slide_Encoder.getPulses();
+        Slide_Revolutions = Slide_Counts /(32*131);
+        Slide_Angle =       Slide_Revolutions*2*pi;
+        Slide_Position =    Slide_Angle*Slide_Radius + 135;
+        
+        Lift_Counts =       Lift_Encoder.getPulses();
+        Lift_Revolutions =  Lift_Counts /(32*131);
+        Lift_Angle =        Lift_Revolutions*2*pi;
+        Lift_Position =     Lift_Angle*Lift_Radius;
+        //pc.printf("\n\r%f", Lift_Counts);
+        
+        if (Controller_Flag == true){
+            Slide_Controller();
+            Lift_Controller();
+        }
+        
+        if (pc.readable()) {
+            Key = pc.getc();
+            switch(Key) {                         //Check to see which Key key...
+                case 0x41:                        //It was the UP Key key...
+                    pc.printf("\n\r UP!");
+                    Lift_Input_force = -1;
+                    break;
+                case 0x42:                        //It was the DOWN Key key...
+                    pc.printf("\n\r DOWN!");
+                    Lift_Input_force = 1;
+                    break;
+                case 0x43:                        //It was the RIGHT Key key...
+                    //pc.printf("\n\r RIGHT!");
+                     Slide_Input_force = 1;   
+                    break;
+                case 0x44:                        //It was the LEFT Key key...
+                    //pc.printf("\n\r LEFT!");
+                    Slide_Input_force = -1; 
+                    break;
+            }
+        } else { 
+            Slide_Input_force = 0;
+            wait(1/Frequency);
+        }   
         //Motor1_pwm.write(Slide_Controller(Input_force));
         
-        wait(1/Frequency);
     }
     return 0;
 }
-
-
-
-
-float Position(float Angle){
-    return Angle*Radius;
+void Ticker_Flag(){
+    Controller_Flag = true;
 }
 
 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
     
-    Desired_speed = Input_force*Multiplier;
-    
-    if (Position(Angle) < Start_slow && Desired_speed < 0){
-        Desired_speed *= (Position(Angle)-Start_lock)/(Start_slow-Start_lock)*1.5-0.5;
+    Slide_Desired_speed= Slide_Input_force*Slide_Multiplier;
+   
+    if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
+        Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
+        
     }
-    if (Position(Angle) > End_slow && Desired_speed > 0){
-        Desired_speed *= (End_lock-Position(Angle))/(End_lock-End_slow)*1.5-0.5;
+    if (Slide_Position > End_slow && Slide_Desired_speed < 0){
+       Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
     }
-    Prev_delta_speed = Delta_speed;
-    Delta_speed = Desired_speed-Curr_speed;                         // P
-    Int_delta_speed += Delta_speed/Frequency;                       // I
-    Deriv_delta_speed = (Delta_speed-Prev_delta_speed)*Frequency;   // D
-    Motor1_pwm.write(k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed);
-    //return k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed;
+    Slide_Prev_delta_speed = Slide_Delta_speed;
+    Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
+    Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
+    
+    Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
+    if (Slide_PI<0){
+        SlideMotor_Direction = 0;
+    }else{
+        SlideMotor_Direction = 1;
+    }   
+    
+    Motor1_pwm.write(abs(Slide_PI));
+    //return k1*Delta_speed + k2*Int_delta_speed;
+}
+
+void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
+    
+    Lift_Desired_position = (Potmeter.read()*2-1)*30*Lift_Multiplier;
+     pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
+    if (Lift_Desired_position < Lift_Start){
+      // Lift_Desired_position = Lift_Start;
+    }
+    if (Lift_Desired_position > Lift_End){
+     //  Lift_Desired_position = Lift_End;
+    }
+    Lift_Prev_delta_position = Lift_Delta_position;                             
+    Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
+    Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
+    Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;
+    
+    Lift_PI = Lift_k1*Lift_Delta_position + Lift_k2*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
+    if (Lift_PI<0){
+        LiftMotor_Direction = 1;
+    }else{
+        LiftMotor_Direction = 0;
+    }   
+    
+    Motor2_pwm.write(abs(Lift_PI));
+    //return k1*Delta_speed + k2*Int_delta_speed;
 }
\ No newline at end of file