Diff: main.cpp

Revision:

0:24628832f21d

Child:

1:41a9e3340c96

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 01 14:33:20 2015 +0000
@@ -0,0 +1,175 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+
+//////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
+//info uit
+HIDScope scope(5);
+
+//encoders
+QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
+QEI encoder2 (D10,D11,NC,32);
+
+//ingangen
+DigitalIn butR(D2);
+DigitalIn butL(D3);
+
+//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
+
+//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 read_but_frequency=25;
+
+//tickers
+Ticker hidscope_ticker;
+Ticker pi_control_ticker;
+Ticker read_but_ticker;
+
+//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////////////////////////////////
+
+//DEZE WAARDES ZIJN ZOMAAR RANDOM WAARDES!!!!
+
+
+double desiredangle[2]= {0,0};
+double desiredposition=0;
+double signal_motor[2]= {0,0};
+////////////////////////////////////////////GO FLAGS AND ACTIVATION FUNCTIONS//////////////////////////////////
+//go flags
+volatile bool scopedata_go=false, pi_control_go=false,read_but_go=false;
+
+//acvitator functions
+
+void scopedata_activate()
+{
+    scopedata_go=true;
+}
+void pi_control_activate()
+{
+    pi_control_go=true;
+}
+void read_but_activate()
+{
+    read_but_go=true;
+}
+///////////////////////////////////////////////////////FUNCTIONS//////////////////////////////////////////////////////////////////////////
+
+//scopedata
+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.send();
+}
+//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
+float pi_kp=0.5;
+float pi_ki=0.01;
+float Ts=1.0/pi_control_frequency;
+double e[2]= {0,0};
+double motor_error_int[2]= {0,0};
+
+// Reusable PI controller
+void pi_control()
+{
+    motor_error_int[0] = motor_error_int[0] + Ts * e[0]; // e_int is changed globally because it’s ’by reference’ (&)
+    signal_motor[0]=pi_kp*e[0]+pi_ki*motor_error_int[0];
+
+    motor_error_int[1] = motor_error_int[1] + Ts * e[1]; // e_int is changed globally because it’s ’by reference’ (&)
+    signal_motor[1]=pi_kp*e[1]+pi_ki*motor_error_int[1];
+}
+
+//////////////////////////////////////////////////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);
+    read_but_ticker.attach(&read_but_activate,1.0/read_but_frequency);
+
+    while(1) {
+        //control motor 1 with a pi controller
+
+        if (read_but_go==true) {
+            float buttonR=butR.read(), buttonL=butL.read();
+            if (buttonR==0 && buttonL ==1) {
+                desiredposition += (radpersecbut);
+                read_but_go=false;
+            }
+            if (buttonR==1 && buttonL==0) {
+                desiredposition -= (radpersecbut);
+                read_but_go=false;
+            } else {
+                desiredposition=desiredposition;
+            }
+            desiredangle[0]=desiredposition;
+            desiredangle[1]=desiredposition;
+            read_but_go=false;
+        }
+
+        if (pi_control_go==true) {
+            e[0]=desiredangle[0]-(counttorad*encoder1.getPulses());
+            e[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;
+        }
+        //call scopedata
+        if (scopedata_go==true) {
+            scopedata();
+            scopedata_go=false;
+        }
+    }
+    return 0;
+}