Diff: main.cpp

Revision:

0:4141aef83f4b

Child:

1:d7299175a12e

diff -r 000000000000 -r 4141aef83f4b main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 01 12:55:50 2017 +0000
@@ -0,0 +1,195 @@
+#include "QEI.h"
+#include "math.h"
+#include "mbed.h"
+//#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+
+//left pot to set reference position.
+//right pot to set Kp, right pot sets Ki when (right)button is pressed down
+
+
+//--------------object creation------------------ 
+Serial pc(USBTX, USBRX);
+//Use X4 encoding.
+//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
+//Use X2 encoding by default.
+QEI enc1(D13, D12, NC, 32); //enable the encoder
+QEI enc2(D15, D14, NC, 32); //enable the encoder
+PwmOut M1_speed(D6);
+PwmOut M2_speed(D5);
+DigitalOut M1_direction(D7);
+DigitalOut M2_direction(D4);
+AnalogIn potmeter(A0);      //left pot
+AnalogIn potmeter2(A1);     //right pot
+InterruptIn button1(D2);       //hardware interrupt for stopping motors
+DigitalIn LimSW1(D1);
+DigitalIn LimSW2(D0);
+DigitalIn HomStart(D3);
+Ticker motor_update;
+BiQuad bqlowpass(0,    0.611,    0.611,  1,    0.222);
+//create scope objects - note: script won't run when HID usb port is not connected
+//HIDScope scope(5); //set # of channels
+Ticker scopeTimer;
+
+
+//-----------------variable decleration----------------
+int pwm_freq = 500;
+float set_speed;
+float reference_pos;
+
+
+float Arm1_home = 112.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
+float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base
+
+double M1_home; 
+double M1_error_pos = 0;
+float M1_Kp = 0.1;
+float M1_Ki = 0.1;
+float M1_Kd = 0.1;
+double M1_e_int=0;
+double M1_e_prior=0;
+
+double M2_home; 
+double M2_error_pos = 0;
+float M2_Kp = 0.1;
+float M2_Ki = 0.1;
+float M2_Kd = 0.1;
+double M2_e_int=0;
+double M2_e_prior=0;
+
+float Ts = 0.002; //500hz sample freq
+
+ 
+ void homing_system () {
+    LimSW1.mode(PullDown);
+    LimSW2.mode(PullDown);
+    M1_speed.write(0);
+    M2_speed.write(0);
+    M1_direction.write(0);
+    M2_direction.write(0);
+    
+    while (true){
+   
+        if (HomStart == 0){
+          M1_speed.write(0.1);
+          }  
+ 
+        if (LimSW1 ==  1){
+            M1_speed.write(0);
+            M2_speed.write(0.1);
+            M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+            }
+        if (LimSW2 == 1) {
+            M2_speed.write(0);
+            M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
+            }
+            
+            if (LimSW1 == 1 && LimSW2 ==1) {
+                break;
+                }
+}
+}
+
+ 
+void kinemtaica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
+  double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
+  double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q2))/(0.12*sin(q2))*vdy;
+  q1_new = q1 +q1_dot*Ts;
+  q2_new = q2 +q2_dot*Ts;
+ return;
+}
+
+float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
+    e_int += Ts*e;
+    double e_diff = (e-e_prior)/Ts;
+    e_prior = e;
+    double e_diff_filter = bqlowpass.step(e_diff);
+    return(Kp*e+Ki*e_int+Kd*e_diff_filter);
+    }
+
+void M1_control(){
+    
+    //call PID func and set new motor speed
+    set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
+    if(set_speed > 0){
+     M1_speed.write(abs(set_speed));
+     M1_direction.write(0);
+     }
+    else if (set_speed < 0){
+     M1_speed.write(abs(set_speed));
+     M1_direction.write(1);
+     }
+    else{M1_speed.write(0);}
+ }
+ 
+ void M2_control(){
+    set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
+    if(set_speed > 0){
+     M2_speed.write(abs(set_speed));
+     M2_direction.write(0);
+     }
+    else if (set_speed < 0){
+     M2_speed.write(abs(set_speed));
+     M2_direction.write(1);
+     }
+    else{M2_speed.write(0);}       
+  }
+ 
+
+ 
+ void scopesend(){
+  
+     
+     
+     }
+ void StopMotors(){
+     while(1){
+         M1_speed.write(0);
+         M2_speed.write(0);
+         }
+    }
+ 
+void geterror(){ 
+    double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
+    double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
+    
+    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
+    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
+    
+    double q1 = M1_actual_pos;
+    double q2 = 3.1416-M1_acual_pos-M2_actual_pos //see drawing
+    
+    double M1_reference_pos = potmeter.read();
+    double M2_reference_pos = potmeter2.read();
+        
+    M1_error_pos = M1_reference_pos - M1_actual_pos;
+    M2_error_pos = M2_reference_pos - M2_actual_pos;
+}
+ 
+int main() {
+    
+    //initialize serial comm and set motor PWM freq
+M1_speed.period(1.0/pwm_freq);
+M2_speed.period(1.0/pwm_freq);
+pc.baud(115200);
+//commence homing procedure
+homing_system();
+//attach all interrupt
+button1.fall(StopMotors);     //stop motor interrupt
+motor_update.attach(&M1_control, &M2_control,0.01);
+//motor_update.attach(&M2_control,0.01);
+
+
+
+
+
+
+pc.printf("initialization complete \n\r");
+    
+    while(1){   
+        
+     
+    }
+ 
+}
\ No newline at end of file