Diff: main.cpp

Revision:

0:6bebd74a5238

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 30 15:14:28 2015 +0000
@@ -0,0 +1,141 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "HIDScope.h"
+
+//////////////////////////////////////CONSTANTS, I/O, FREQUENCIES AND TICKERS//////////////////////////////////////
+//info uit
+HIDScope scope(3);
+
+//encoders
+QEI encoder1 (D12,D13,NC,32); // first b then a for clockwise +
+
+//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 int cpr=32*131;
+const float PI=3.1415;
+const float counttorad=((2*PI)/cpr);
+const float counttodeg=(360/cpr);
+const float mmpersecbut=(10/(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!!!!
+float pi_kp=0.5;
+float pi_ki=0.01;
+float Ts=1.0/pi_control_frequency;
+double motor1_error_int=0;
+double desiredangle=0;
+double desiredposition=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,desiredangle);//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.send();
+}
+//////////////////////////////////////////////////////////CONTROLLER///////////////////////////////////////
+
+// Reusable PI controller
+double pi_control( double e, double& motor1_error_int)
+{
+    motor1_error_int = motor1_error_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
+    return pi_kp*e+pi_ki*motor1_error_int;
+}
+
+//////////////////////////////////////////////////MAIN///////////////////////////////////
+int main()
+{
+    //set initial shizzle
+    motor1_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=desiredposition;
+            read_but_go=false;
+        }
+        
+        if (pi_control_go==true) {
+            double error=desiredangle-(counttorad*encoder1.getPulses());
+            double signal_motor1=pi_control(error,motor1_error_int);//send error to p controller
+            if (signal_motor1>=0) {//determine CW or CCW rotation
+                motor1_direction.write(CW);
+            } else {
+                motor1_direction.write(CCW);
+            }
+
+            if (fabs(signal_motor1)>=1) { //check if signal is <1
+                signal_motor1=1;//if signal >1 make it 1 to not damage motor
+            } else {
+                signal_motor1=fabs(signal_motor1);// if signal<1 use signal
+            }
+
+            motor1_speed_control.write(fabs(signal_motor1));//write signal to motor
+            pi_control_go=false;
+        }
+        //call scopedata
+        if (scopedata_go==true) {
+            scopedata();
+            scopedata_go=false;
+        }
+    }
+    return 0;
+}