Diff: main.cpp

Revision:

13:3482d315877c

Parent:

10:7339dca7d604

--- a/main.cpp	Tue Oct 23 08:44:17 2018 +0000
+++ b/main.cpp	Mon Oct 29 12:55:38 2018 +0000
@@ -1,144 +1,156 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "QEI.h"
-#include "HIDScope.h"
 #include "BiQuad.h"
-#include "PID_controller.h"
-#include "kinematics.h"
-
-//Define objects
-MODSERIAL pc(USBTX, USBRX);
-HIDScope    scope(2);
-
-// emg inputs
-AnalogIn    emg0( A0 );
-AnalogIn    emg1( A1 );
 
-// motor ouptuts
-PwmOut      motor1_pwm(D5);
-DigitalOut  motor1_dir(D4);
-PwmOut      motor2_pwm(D7);
-DigitalOut  motor2_dir(D6);
-
-AnalogIn    potmeter1(A2);
-AnalogIn    potmeter2(A3);
-DigitalIn   button(D0);
-
-Ticker Sample;
-Timer state_timer;
+// CHECK
+PwmOut pwmpin(D6);
+PwmOut pwmpin2(D5);
+DigitalOut directionpin2(D4);
+DigitalOut directionpin(D7);
+//MODSERIAL pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
 
-enum States {failure, waiting, calib_enc, calib_emg, operational, demo}; //All possible robot states
-
-//Global variables/objects
-States current_state;
-Ticker loop_ticker; //The Ticker object that will ensure perfect timing of our looping code
-float e, u1, u2, emg_signal_raw_0, processed_emg_0, emg_signal_raw_1, processed_emg_1, robot_end_point, reference_end_point, motor_angle, motor_counts, q_ref; //will be set by the motor_controller function
-int counts_per_rotation = 32;
-bool state_changed = false;
-double samplingfreq = 1000;
-double x; // Making the position (x,y) of the end effector global
-double y;
+QEI wheel1 (D13, D12, NC, 32);
+QEI wheel2 (D11, D10, NC, 32);
+float u1,u2 = 0;
 
-float processing_chain_emg(int num) {
-    return 6.0;
-}
+// for trajectory control:
+double T = 1; // time to get to destination
+double currentx, currenty;
+double currentq1, currentq2;
+// end    
 
-void measure_all() 
-{
-    motor_angle = motor_counts*2.0f*3.1415926535f/counts_per_rotation; //do this here, and not in the encoder interrupt, to reduce computational load
-    robot_end_point = forwardkinematics_function(motor_angle);  //motor_angle is global, this function ne
-    emg_signal_raw_0 = emg0.read(); //sample analog voltages (all sampling theory applies, you might get aliasing etc.)
-    emg_signal_raw_1 = emg1.read();
-    processed_emg_0 = processing_chain_emg(0);  // some function ‘float my_emg_processing_chain()’ that returns a float. The raw emg is global
-    processed_emg_1 = processing_chain_emg(1);
-}
 
-void output_all() {
-    motor1_pwm = fabs(u1);
-    motor1_dir = u1 > 0.5f;
-    motor2_pwm = fabs(u2);
-    motor2_dir = u2 > 0.5f;
-    static int output_counter = 0;
-    output_counter++;
-    if (output_counter == 100) {pc.printf("Something something... %f",u1); output_counter = 0;}  //Print to screen at 10 Hz with MODSERIAL
-}
+float angle_resolution = (360.0/32.0)*(1.0/131.0);  //degrees/counts 
+double Kp = 2;
+double Ki = 1.02;
+double Kd = 10;
+extern double samplingfreq = 1000;
+ 
+double L1 = 0.43;
+double L2 = 0.43;
+double x01 = 0.0;
+double y01 = 0.0;
 
-void state_machine() {
-    switch(current_state) { //States can be: failure, wait, calib_enc, calib_emg, operational, demo,
-        case waiting:  //Nothing useful here, maybe a blinking LED for fun and communication to the user
-            if (button.read()==true) 
-            {
-                current_state = calib_enc; //the NEXT loop we will be in calib_enc state
-            }
-            break; //to avoid falling through to the next state, although this can sometimes be very useful.
-        
-        case calib_enc:
-            if (state_changed==true) 
-            {
-                state_timer.reset();
-                state_timer.start();
-                state_changed = false;
-            }
-            u1 = 0.6f; //a low PWM value to move the motors slowly (0.0 to 0.45 don’t do much due to friction)
-            // fabs(motor1.velocity()) < 0.1f && 
-            if (state_timer.read() > 5.0f) {
-                current_state = calib_emg; //the NEXT loop we will be in calib_emg state
-                state_changed = true;
-            }
-            break;
-            
-        case calib_emg:     //calibrate emg-signals
-            
-            break;
-        
-        case operational:       //interpreting emg-signals to move the end effector
-            if (state_changed==true) { int x = 5; }
-            // example
-            reference_end_point = robot_end_point + processed_emg_0;
-            if (button.read() == true) { current_state = demo; }
-            
-            break;
-            
-        case demo: //moving according to a specified trajectory
-            
-            if (button.read() == true) { current_state = demo; }
-            
-            break;
-        
-        case failure: //no way to get out
-            u1 = 0.0f;
-            break;
-    }
+void forwardkinematics_function(double& q1, double& q2) {
+    // input are joint angles, output are x and y position of end effector
+    
+    currentx = x01 + L1*cos(q1)-L2*cos(q2);
+    currenty = y01 + L1 * sin(q1) - L2 * sin(q2);    
 }
-
-void motor_controller() 
-{
-    if (current_state >= operational) { // we can (ab)use the fact that an enum is actually an integer, so math/logic rules still apply
-        q_ref += inversekinematics_function(reference_end_point)/samplingfreq; //many different states can modify your reference position, so just do the inverse kinematics once, here
-        e1 = q_ref - motor_angle; //tracking error (q_ref - q_meas)
-        e2 = q_ref - motor_angle;
-        PID_controller(e1,e2,u1,u2); //feedback controller or with possibly fancy controller additions...; pass by reference
-        } //otherwise we just don’t mess with the value of control variable ‘u’ that is set somewhere in the state-machine.
+ 
+vector<double> inversekinematics_function(double& x, double& y, const double& T, double& q1, double& q2, double& des_vx, double& des_vy) {
+    // x, y: positions of end effector | T: time to get to position | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
+    
+    // pseudo inverse jacobian to get joint speeds
+    // input are desired vx and vy of end effector, output joint angle speeds
+ 
+    double q1_star_des; // desired joint velocity of q1_star
+    double q2_star_des; // same as above but then for q2_star
+    double qref1, qref2;
+ 
+    // The calculation below assumes that the end effector position is calculated before this function is executed
+    // In our case the determinant will not equal zero, hence no problems with singularies I think.
+    q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
+    q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy); // CHECK THIS ONE!!!
+    
+    qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
+    qref2 = q2+T*(q2_star_des - q1_star_des); 
+    
+    vector<double> thetas;
+    thetas.push_back(qref1);
+    thetas.push_back(qref2);
+    return thetas;
 }
 
 
-void loop_function() {
-    measure_all();              //measure all signals
-    state_machine();            //Do relevant state dependent things
-    motor_controller();         //Do not put different motor controllers in the states, because every state can re-use the same motor-controller!
-    output_all();               //Output relevant signals, messages, screen outputs, LEDs etc.
+void PID_controller(double error1, double error2, float &u1, float &u2)
+{   
+    double u_k = Kp * error1;
+    double u_k2 = Kp * error2;
+    static double error_integral = 0;
+    static double error_prev = error1; // initialization with this value only done once!
+    static double error_prev2 = error2; // initialization with this value only done once!
+
+    static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
+    
+    error_integral = error_integral + error1 * 1/samplingfreq;
+    double u_i = Ki * error_integral;
+    double error_derivative = (error1 - error_prev)*samplingfreq;
+    double filtered_error_derivative = LowPassFilter.step(error_derivative);
+    double u_d = Kd * filtered_error_derivative;
+    error_prev = error1;
+    error_prev2 = error2; 
+    u1 =  float(u_k/360);//+u_i+u_d;
+    u2 = float(u_k2/360);
+    
+   
+   
 }
 
+int main()
+{   
+    pwmpin.period_us(60);
+    int pulses1, pulses2 = 0;
+    float angle1, angle2;
+    int realangle1, realangle2;
+    double ref1, error1;
+    double ref2, error2;
+    bool reached;
+    
+    double vx, vy;
+    
+    while (true) {
+        pulses1 = wheel1.getPulses();
+        angle1 = pulses1*angle_resolution;
+        realangle1 = abs(int(angle1)) % 360;
+        
+        pulses2 = wheel2.getPulses();
+        angle2 = pulses2*angle_resolution;
+        realangle2 = abs(int(angle2)) % 360;
+        
+        currentq1 = angle1;
+        currentq2 = angle2;
+        
+        //forwardkinematics_function(currentq1, currentq2);
+        T = 2; // 2 seconds seems slow enough :D
+        vx = 0.1;
+        vy = 0;
+    
+        vector<double> refangles = inversekinematics_function(currentx, currenty, T, currentq1, currentq2, vx, vy);
+        
+        ref1 = 0;
+        ref2 = 0;
+        
+        error1 = ref1 - realangle1;
+        error2 = ref2 - realangle2;
+        
+        
+        //PID_controller(error1, error2, u1, u2); 
+        if(u1 > 1){
+            u1 = 1;
+        }
+        if(u1 < -1){
+            u1 = -1;
+            }
+        if(u2 > 1){
+            u2 = 1;
+        }
+        if(u2 < -1){
+            u2 = -1;
+            } 
 
-int main() 
-{
-    pc.baud(115200);
-    motor1_pwm.period_us(60);
-    motor2_pwm.period_us(60);
-    current_state = waiting; //we start in state ‘waiting’ and current_state can be accessed by all functions
-    u1 = 0.0f; //initial output to motors is 0.
-    u2 = 0.0f;
-    loop_ticker.attach(&loop_function, 1/samplingfreq); //Run the function loop_function 1000 times per second
-    
-    while (true) { }  //Do nothing here (timing purposes)
-}
+            
+        pwmpin = fabs(u1);
+        pwmpin2 = fabs(u2);
+         
+        // 1/true is negative, 0/false is positive 
+        directionpin2 = u2 < 0;
+        directionpin = u1 < 0;
+        pc.printf("angle %f, error: %f, pwm: %f \r \n",realangle1, error1, u1);
+        
+         
+        
+    }
+}
\ No newline at end of file