Biorobotics / Robot-Software_jesse

helloworld

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

main.cpp@13:3482d315877c, 2018-10-29 (annotated)

Committer:
Peppypeppy
Date:
Mon Oct 29 12:55:38 2018 +0000
Revision:
13:3482d315877c
Parent:
10:7339dca7d604 
hello;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2 #include "MODSERIAL.h"
MaikOvermars 0:4cb1de41d049 3 #include "QEI.h"
MaikOvermars 0:4cb1de41d049 4 #include "BiQuad.h"
MaikOvermars 0:4cb1de41d049 5
Peppypeppy 13:3482d315877c 6 // CHECK
Peppypeppy 13:3482d315877c 7 PwmOut pwmpin(D6);
Peppypeppy 13:3482d315877c 8 PwmOut pwmpin2(D5);
Peppypeppy 13:3482d315877c 9 DigitalOut directionpin2(D4);
Peppypeppy 13:3482d315877c 10 DigitalOut directionpin(D7);
Peppypeppy 13:3482d315877c 11 //MODSERIAL pc(USBTX, USBRX);
Peppypeppy 13:3482d315877c 12 Serial pc(USBTX, USBRX);
MaikOvermars 0:4cb1de41d049 13
Peppypeppy 13:3482d315877c 14 QEI wheel1 (D13, D12, NC, 32);
Peppypeppy 13:3482d315877c 15 QEI wheel2 (D11, D10, NC, 32);
Peppypeppy 13:3482d315877c 16 float u1,u2 = 0;
MaikOvermars 0:4cb1de41d049 17
Peppypeppy 13:3482d315877c 18 // for trajectory control:
Peppypeppy 13:3482d315877c 19 double T = 1; // time to get to destination
Peppypeppy 13:3482d315877c 20 double currentx, currenty;
Peppypeppy 13:3482d315877c 21 double currentq1, currentq2;
Peppypeppy 13:3482d315877c 22 // end
MaikOvermars 0:4cb1de41d049 23
MaikOvermars 0:4cb1de41d049 24
Peppypeppy 13:3482d315877c 25 float angle_resolution = (360.0/32.0)*(1.0/131.0); //degrees/counts
Peppypeppy 13:3482d315877c 26 double Kp = 2;
Peppypeppy 13:3482d315877c 27 double Ki = 1.02;
Peppypeppy 13:3482d315877c 28 double Kd = 10;
Peppypeppy 13:3482d315877c 29 extern double samplingfreq = 1000;
Peppypeppy 13:3482d315877c 30
Peppypeppy 13:3482d315877c 31 double L1 = 0.43;
Peppypeppy 13:3482d315877c 32 double L2 = 0.43;
Peppypeppy 13:3482d315877c 33 double x01 = 0.0;
Peppypeppy 13:3482d315877c 34 double y01 = 0.0;
MaikOvermars 0:4cb1de41d049 35
Peppypeppy 13:3482d315877c 36 void forwardkinematics_function(double& q1, double& q2) {
Peppypeppy 13:3482d315877c 37 // input are joint angles, output are x and y position of end effector
Peppypeppy 13:3482d315877c 38
Peppypeppy 13:3482d315877c 39 currentx = x01 + L1*cos(q1)-L2*cos(q2);
Peppypeppy 13:3482d315877c 40 currenty = y01 + L1 * sin(q1) - L2 * sin(q2);
MaikOvermars 0:4cb1de41d049 41 }
Peppypeppy 13:3482d315877c 42
Peppypeppy 13:3482d315877c 43 vector<double> inversekinematics_function(double& x, double& y, const double& T, double& q1, double& q2, double& des_vx, double& des_vy) {
Peppypeppy 13:3482d315877c 44 // 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
Peppypeppy 13:3482d315877c 45
Peppypeppy 13:3482d315877c 46 // pseudo inverse jacobian to get joint speeds
Peppypeppy 13:3482d315877c 47 // input are desired vx and vy of end effector, output joint angle speeds
Peppypeppy 13:3482d315877c 48
Peppypeppy 13:3482d315877c 49 double q1_star_des; // desired joint velocity of q1_star
Peppypeppy 13:3482d315877c 50 double q2_star_des; // same as above but then for q2_star
Peppypeppy 13:3482d315877c 51 double qref1, qref2;
Peppypeppy 13:3482d315877c 52
Peppypeppy 13:3482d315877c 53 // The calculation below assumes that the end effector position is calculated before this function is executed
Peppypeppy 13:3482d315877c 54 // In our case the determinant will not equal zero, hence no problems with singularies I think.
Peppypeppy 13:3482d315877c 55 q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
Peppypeppy 13:3482d315877c 56 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!!!
Peppypeppy 13:3482d315877c 57
Peppypeppy 13:3482d315877c 58 qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
Peppypeppy 13:3482d315877c 59 qref2 = q2+T*(q2_star_des - q1_star_des);
Peppypeppy 13:3482d315877c 60
Peppypeppy 13:3482d315877c 61 vector<double> thetas;
Peppypeppy 13:3482d315877c 62 thetas.push_back(qref1);
Peppypeppy 13:3482d315877c 63 thetas.push_back(qref2);
Peppypeppy 13:3482d315877c 64 return thetas;
MaikOvermars 0:4cb1de41d049 65 }
MaikOvermars 0:4cb1de41d049 66
MaikOvermars 0:4cb1de41d049 67
Peppypeppy 13:3482d315877c 68 void PID_controller(double error1, double error2, float &u1, float &u2)
Peppypeppy 13:3482d315877c 69 {
Peppypeppy 13:3482d315877c 70 double u_k = Kp * error1;
Peppypeppy 13:3482d315877c 71 double u_k2 = Kp * error2;
Peppypeppy 13:3482d315877c 72 static double error_integral = 0;
Peppypeppy 13:3482d315877c 73 static double error_prev = error1; // initialization with this value only done once!
Peppypeppy 13:3482d315877c 74 static double error_prev2 = error2; // initialization with this value only done once!
Peppypeppy 13:3482d315877c 75
Peppypeppy 13:3482d315877c 76 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Peppypeppy 13:3482d315877c 77
Peppypeppy 13:3482d315877c 78 error_integral = error_integral + error1 * 1/samplingfreq;
Peppypeppy 13:3482d315877c 79 double u_i = Ki * error_integral;
Peppypeppy 13:3482d315877c 80 double error_derivative = (error1 - error_prev)*samplingfreq;
Peppypeppy 13:3482d315877c 81 double filtered_error_derivative = LowPassFilter.step(error_derivative);
Peppypeppy 13:3482d315877c 82 double u_d = Kd * filtered_error_derivative;
Peppypeppy 13:3482d315877c 83 error_prev = error1;
Peppypeppy 13:3482d315877c 84 error_prev2 = error2;
Peppypeppy 13:3482d315877c 85 u1 = float(u_k/360);//+u_i+u_d;
Peppypeppy 13:3482d315877c 86 u2 = float(u_k2/360);
Peppypeppy 13:3482d315877c 87
Peppypeppy 13:3482d315877c 88
Peppypeppy 13:3482d315877c 89
MaikOvermars 0:4cb1de41d049 90 }
MaikOvermars 0:4cb1de41d049 91
Peppypeppy 13:3482d315877c 92 int main()
Peppypeppy 13:3482d315877c 93 {
Peppypeppy 13:3482d315877c 94 pwmpin.period_us(60);
Peppypeppy 13:3482d315877c 95 int pulses1, pulses2 = 0;
Peppypeppy 13:3482d315877c 96 float angle1, angle2;
Peppypeppy 13:3482d315877c 97 int realangle1, realangle2;
Peppypeppy 13:3482d315877c 98 double ref1, error1;
Peppypeppy 13:3482d315877c 99 double ref2, error2;
Peppypeppy 13:3482d315877c 100 bool reached;
Peppypeppy 13:3482d315877c 101
Peppypeppy 13:3482d315877c 102 double vx, vy;
Peppypeppy 13:3482d315877c 103
Peppypeppy 13:3482d315877c 104 while (true) {
Peppypeppy 13:3482d315877c 105 pulses1 = wheel1.getPulses();
Peppypeppy 13:3482d315877c 106 angle1 = pulses1*angle_resolution;
Peppypeppy 13:3482d315877c 107 realangle1 = abs(int(angle1)) % 360;
Peppypeppy 13:3482d315877c 108
Peppypeppy 13:3482d315877c 109 pulses2 = wheel2.getPulses();
Peppypeppy 13:3482d315877c 110 angle2 = pulses2*angle_resolution;
Peppypeppy 13:3482d315877c 111 realangle2 = abs(int(angle2)) % 360;
Peppypeppy 13:3482d315877c 112
Peppypeppy 13:3482d315877c 113 currentq1 = angle1;
Peppypeppy 13:3482d315877c 114 currentq2 = angle2;
Peppypeppy 13:3482d315877c 115
Peppypeppy 13:3482d315877c 116 //forwardkinematics_function(currentq1, currentq2);
Peppypeppy 13:3482d315877c 117 T = 2; // 2 seconds seems slow enough :D
Peppypeppy 13:3482d315877c 118 vx = 0.1;
Peppypeppy 13:3482d315877c 119 vy = 0;
Peppypeppy 13:3482d315877c 120
Peppypeppy 13:3482d315877c 121 vector<double> refangles = inversekinematics_function(currentx, currenty, T, currentq1, currentq2, vx, vy);
Peppypeppy 13:3482d315877c 122
Peppypeppy 13:3482d315877c 123 ref1 = 0;
Peppypeppy 13:3482d315877c 124 ref2 = 0;
Peppypeppy 13:3482d315877c 125
Peppypeppy 13:3482d315877c 126 error1 = ref1 - realangle1;
Peppypeppy 13:3482d315877c 127 error2 = ref2 - realangle2;
Peppypeppy 13:3482d315877c 128
Peppypeppy 13:3482d315877c 129
Peppypeppy 13:3482d315877c 130 //PID_controller(error1, error2, u1, u2);
Peppypeppy 13:3482d315877c 131 if(u1 > 1){
Peppypeppy 13:3482d315877c 132 u1 = 1;
Peppypeppy 13:3482d315877c 133 }
Peppypeppy 13:3482d315877c 134 if(u1 < -1){
Peppypeppy 13:3482d315877c 135 u1 = -1;
Peppypeppy 13:3482d315877c 136 }
Peppypeppy 13:3482d315877c 137 if(u2 > 1){
Peppypeppy 13:3482d315877c 138 u2 = 1;
Peppypeppy 13:3482d315877c 139 }
Peppypeppy 13:3482d315877c 140 if(u2 < -1){
Peppypeppy 13:3482d315877c 141 u2 = -1;
Peppypeppy 13:3482d315877c 142 }
MaikOvermars 0:4cb1de41d049 143
Peppypeppy 13:3482d315877c 144
Peppypeppy 13:3482d315877c 145 pwmpin = fabs(u1);
Peppypeppy 13:3482d315877c 146 pwmpin2 = fabs(u2);
Peppypeppy 13:3482d315877c 147
Peppypeppy 13:3482d315877c 148 // 1/true is negative, 0/false is positive
Peppypeppy 13:3482d315877c 149 directionpin2 = u2 < 0;
Peppypeppy 13:3482d315877c 150 directionpin = u1 < 0;
Peppypeppy 13:3482d315877c 151 pc.printf("angle %f, error: %f, pwm: %f \r \n",realangle1, error1, u1);
Peppypeppy 13:3482d315877c 152
Peppypeppy 13:3482d315877c 153
Peppypeppy 13:3482d315877c 154
Peppypeppy 13:3482d315877c 155 }
Peppypeppy 13:3482d315877c 156 }