Diff: main.cpp

Revision:

2:91bf9f1765ef

Parent:

1:41a9e3340c96

Child:

3:bb7b6034bb7c

diff -r 41a9e3340c96 -r 91bf9f1765ef main.cpp
--- a/main.cpp	Fri Oct 02 09:02:08 2015 +0000
+++ b/main.cpp	Thu Oct 08 13:15:23 2015 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "HIDScope.h"
+#include "controlandadjust.h"
 
 //////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
 //info uit
@@ -14,46 +15,46 @@
 //ingangen
 DigitalIn butR(D2);
 DigitalIn butL(D3);
-InterruptIn changecontrollervalues(PTA4);
-
-//uitgangen
-DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW encoder als je daar op kijkt en CW shaft als je daar op kijkt)
-PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
-PwmOut motor2_speed_control(D5);
-DigitalOut motor2_direction(D4);
-const int CW=1; //clockwise
-const int CCW=0; //counterclockwise
+InterruptIn changecontrollervaluesbutton(PTA4);//button to adjus controllervalues
 
 //frequencies
 const double hidscope_frequency=100;//frequentie waarop data naar HIDScope wordt gestuurd
-const double pi_control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
+const double control_frequency=25;//frequentie waarop er een control actie wordt uitgevoerd. kan hoger, voorzichtig bij ruis!
 const double read_but_frequency=25;
 
 //tickers
 Ticker hidscope_ticker;
-Ticker pi_control_ticker;
+Ticker control_ticker;
 Ticker read_but_ticker;
+Timeout schieten_timeout;
+
+// controller values and errors
+controlandadjust mycontroller;
+const float Kp=0.5;
+const float Ki=0.01;
+const float Kd=0.001;
+const float Ts_control=1.0/control_frequency;
+float error1=0;
+float error2=0;
+float error1_int=0;
+float error2_int=0;
+float error1_prev=0;
+float error2_prec=0;
 
 //constants
 const float cpr=32*131;
 const float PI=3.1415;
 const float counttorad=((2*PI)/cpr);
-const float counttodeg=(360/cpr);
-const float mmpersecbut=(30/(read_but_frequency));
 const float radpersecbut=(PI/read_but_frequency);
-const float armlength=400;//length "upper arm" in mm
-
-///////////////////////////////////////////////////CONTROLLER CONSTANTS////////////////////////////////
+const float schiethoek=0.5*PI;
+const float schiettijd=0.5;
 
-//DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
-
-
-double desiredangle[2]= {0,0};
-double desiredposition=0;
-double signal_motor[2]= {0,0};
+//angle for motor
+float desiredangle[]= {0,0};
+float desiredposition=0;
 ////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
 //go flags
-volatile bool scopedata_go=false, pi_control_go=false,read_but_go=false;
+volatile bool scopedata_go=false, control_go=false,read_but_go=false;
 
 //acvitator functions
 
@@ -61,9 +62,9 @@
 {
     scopedata_go=true;
 }
-void pi_control_activate()
+void control_activate()
 {
-    pi_control_go=true;
+    control_go=true;
 }
 void read_but_activate()
 {
@@ -75,44 +76,47 @@
 void scopedata()
 {
     scope.set(0,desiredposition);//gewenste hoek van arm
-    scope.set(1,counttodeg*encoder1.getPulses());//hoek in rad van outputshaft
-    scope.set(2,motor1_speed_control.read());//pwm signaal naar motor toe
-    scope.set(3,counttodeg*encoder2.getPulses());//hoek in rad van outputshaft
-    scope.set(4,motor2_speed_control.read());//pwm signaal naar motor toe
+    scope.set(1,counttorad*encoder1.getPulses());//hoek in rad van outputshaft
+    scope.set(2,mycontroller.motor1pwm());//pwm signaal naar motor toe
+    scope.set(3,counttorad*encoder2.getPulses());//hoek in rad van outputshaft
+    scope.set(4,mycontroller.motor2pwm());//pwm signaal naar motor toe
     scope.send();
 }
-//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
-float pi_kp=0.5;
-float pi_ki=0.01;
-float Ts=1.0/pi_control_frequency;
-double control_error[2]= {0,0};
-double motor_error_int[2]= {0,0};
+
+
+
+void schieten()
+{
+    pc.printf("SHOOT\n");
+    //hoeken groter maken
+    desiredangle[0]-=schiethoek;
+    desiredangle[1]+=schiethoek;
 
-// Reusable PI controller
-void pi_control()
-{
-    motor_error_int[0] = motor_error_int[0] + Ts * control_error[0]; // e_int is changed globally because it’s ’by reference’ (&)
-    signal_motor[0]=pi_kp*control_error[0]+pi_ki*motor_error_int[0];
+    Timer schiettimer;
+    schiettimer.reset();
+    schiettimer.start();
+    while(schiettimer<=schiettijd) {
+        error1=(desiredangle[0]-counttorad*encoder1.getPulses());
+        error2=(desiredangle[1]-counttorad*encoder2.getPulses());
+        mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int, error2_int);
+    }
+    schiettimer.stop();
 
-    motor_error_int[1] = motor_error_int[1] + Ts * control_error[1]; // e_int is changed globally because it’s ’by reference’ (&)
-    signal_motor[1]=pi_kp*control_error[1]+pi_ki*motor_error_int[1];
+    //terug na schieten
+    desiredangle[0]+=schiethoek;
+    desiredangle[1]-=schiethoek;
 }
 
 //////////////////////////////////////////////////MAIN///////////////////////////////////
 int main()
 {
-    //set initial shizzle
-    motor1_speed_control.write(0);
-    motor2_speed_control.write(0);
-
     //tickers
     hidscope_ticker.attach(&scopedata_activate,1.0/hidscope_frequency);
-    pi_control_ticker.attach(&pi_control_activate,1.0/pi_control_frequency);
+    control_ticker.attach(&control_activate,1.0/control_frequency);
     read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
 
     while(1) {
-        //control motor 1 with a pi controller
-
+        //read buttons and adjust angles
         if (read_but_go==true) {
             float buttonR=butR.read(), buttonL=butL.read();
             if (buttonR==0 && buttonL ==1) {
@@ -122,50 +126,25 @@
             if (buttonR==1 && buttonL==0) {
                 desiredposition -= (radpersecbut);
                 read_but_go=false;
+            }
+            //schieten
+            if (buttonR==0 && buttonL==0) {
+                schieten();
+                read_but_go==false;
             } else {
                 desiredposition=desiredposition;
             }
             desiredangle[0]=desiredposition;
             desiredangle[1]=desiredposition;
+
             read_but_go=false;
         }
-
-        if (pi_control_go==true) {
-            control_error[0]=desiredangle[0]-(counttorad*encoder1.getPulses());
-            control_error[1]=desiredangle[1]-(counttorad*encoder2.getPulses());
-            pi_control();
-            //motor1
-            if (signal_motor[0]>=0) {//determine CW or CCW rotation
-                motor1_direction.write(CW);
-            } else {
-                motor1_direction.write(CCW);
-            }
-
-            if (fabs(signal_motor[0])>=1) { //check if signal is <1
-                signal_motor[0]=1;//if signal >1 make it 1 to not damage motor
-            } else {
-                signal_motor[0]=fabs(signal_motor[0]);// if signal<1 use signal
-            }
-
-            //motor 2
-            if (signal_motor[1]>=0) {//determine CW or CCW rotation
-                motor2_direction.write(CW);
-            } else {
-                motor2_direction.write(CCW);
-            }
-
-            if (fabs(signal_motor[1])>=1) { //check if signal is <1
-                signal_motor[1]=1;//if signal >1 make it 1 to not damage motor
-            } else {
-                signal_motor[1]=fabs(signal_motor[1]);// if signal<1 use signal
-            }
-
-
-
-            motor1_speed_control.write(fabs(signal_motor[0]));//write signal to motor
-            motor2_speed_control.write(fabs(signal_motor[1]));//write signal to motor
-
-            pi_control_go=false;
+//control motors with pi controller
+        if (control_go==true) {
+            error1=(desiredangle[0]-counttorad*encoder1.getPulses());
+            error2=(desiredangle[1]-counttorad*encoder2.getPulses());
+            mycontroller.PI(error1, error2, Kp, Ki, Ts_control, error1_int,error2_int);
+            control_go=false;
         }
         //call scopedata
         if (scopedata_go==true) {
@@ -173,14 +152,16 @@
             scopedata_go=false;
         }
         //unit om controllervalues aan te passen
-        if (changecontrollervalues.read()==0) {
-            motor1_speed_control.write(0);
-            motor2_speed_control.write(0);
-            pc.printf("KP is now %f, enter new value\n",pi_kp);
-            pc.scanf("%f", &pi_kp);
+        if (changecontrollervaluesbutton.read()==0) {
+            mycontroller.STOP();
+            pc.printf("KP is now %f, enter new value\n",Kp);
+            pc.scanf("%f", &Kp);
 
-            pc.printf("KI is now %f, enter new value\n",pi_ki);
-            pc.scanf("%f", &pi_ki);
+            pc.printf("KI is now %f, enter new value\n",Ki);
+            pc.scanf("%f", &Ki);
+
+            pc.printf("KD is now %f, enter new value\n",Kd);
+            pc.scanf("%f", &Kd);
         }
     }
     return 0;