Diff: main.cpp

Revision:

16:75884d07334e

Parent:

14:30d32f51dd9f

--- a/main.cpp	Thu Apr 04 12:25:55 2019 +0000
+++ b/main.cpp	Tue Apr 16 14:23:50 2019 +0000
@@ -6,8 +6,8 @@
 // Algemeen
 DigitalIn button2(SW2);
 DigitalIn button3(SW3);
-AnalogIn But2(A5);
-AnalogIn But1(A3);
+DigitalIn But2(D12);
+DigitalIn But1(D13);
 DigitalOut led(LED_GREEN);
 DigitalOut led2(LED_RED);
 DigitalOut led3(LED_BLUE);
@@ -22,8 +22,8 @@
 volatile float pwm2;
 
 //Encoder
-QEI encoder1 (D10, D9, NC, 1200, QEI::X4_ENCODING);
-QEI encoder2 (D13, D12, NC, 4800, QEI::X4_ENCODING);
+QEI encoder1 (D1, D0, NC, 1200, QEI::X4_ENCODING);
+QEI encoder2 (D3, D2, NC, 4800, QEI::X4_ENCODING);
 double Pulses1;
 double motor_position1;
 double Pulses2;
@@ -31,7 +31,7 @@
 double error1;
 
 //Pot meter
-AnalogIn pot(A1);
+AnalogIn pot(A5);
 AnalogIn pot0(A0);
 float Pot2;
 float Pot1;
@@ -76,19 +76,23 @@
 double Kp2 = 6;          // Zonder arm: 6,0,1
 double Ki2 = 0;          
 double Kd2 = 1;          
-double Ts = 0.0005;      // Sample time in seconds
+double Ts = 0.00006;      // Sample time in seconds
+
+float countpwm = 0;
+
+
 
 float Kinematics1(float KPot)
 {
 
     if (KPot > 0.45f) {
-        stap1 = KPot*450*Ts;
+        stap1 = KPot*450*0.000001;
         Hoeknieuw1 = PolsReference + stap1;
         return Hoeknieuw1;
     }
 
     else if (KPot < -0.45f) {
-        stap1 = KPot*450*Ts;
+        stap1 = KPot*450*0.000001;
         Hoeknieuw1 = PolsReference + stap1;
         return Hoeknieuw1;
     }
@@ -276,13 +280,47 @@
 
 }
 
+void Servod(void)
+{
+     if (countpwm == 0 ){
+      //Reference hoek berekenen, in graden
+    float Ellebooghoek1 = Kinematics2(pwm2);
+    float Ellebooghoek4 = Limits2(Ellebooghoek1);
+    //ElbowReference = Ellebooghoek4;
+    
+    //float Polshoek1 = Kinematics1(pwm2);
+    //float Polshoek4 = Limits1(Polshoek1);
+    //PolsReference = Polshoek4;
+
+    // Positie motor berekenen, in graden
+    Pulses1 = encoder1.getPulses();
+    motor_position1 = -(Pulses1/1200)*360;
+    Pulses2 = encoder2.getPulses();
+    motor_position2 = -(Pulses2/4800)*360;
+
+    double error1 = PolsReference - motor_position1;
+    double u1 = PID_controller1(error1);
+    moter1_control(u1);
+    
+    double error2 = ElbowReference - motor_position2;
+    double u2 = PID_controller2(error2);
+    moter2_control(u2);
+      }   
+     else if (countpwm == 9){
+      countpwm = 0;
+     } 
+     led3 = !led3;
+}
+     
 void MotorOn(void)
 {
     pwmpin1 = 0;
     pwmpin2 = 0;
-    Pwm.attach (PwmMotor, Ts);
+    //Pwm.attach (PwmMotor, Ts);
+    Pwm.attach (Servod, Ts);
 
 }
+
 void MotorOff(void)
 {
     Pwm.detach ();
@@ -317,7 +355,7 @@
     t.start();
     int counter = 0;
     pwmpin2.period_us(60);
-    PotRead.attach(ContinuousReader,Ts);
+    PotRead.attach(ContinuousReader,0.0005);
     Kdc.attach(Kdcount,5);                //Voor PID waarde testen
     pc.baud(115200);
     //pc.printf("start\r\n");