Biorobotics / DemoMode

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software_jesse by Biorobotics

main.cpp@14:c4dbce7de430, 2018-10-31 (annotated)

Committer:
Spekdon
Date:
Wed Oct 31 20:19:01 2018 +0000
Revision:
14:c4dbce7de430
Parent:
13:3482d315877c 
fixed everything (except for a few things)

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"
Spekdon 14:c4dbce7de430 5 #include "math.h"
Spekdon 14:c4dbce7de430 6 #include <vector>
MaikOvermars 0:4cb1de41d049 7
Peppypeppy 13:3482d315877c 8 // CHECK
Peppypeppy 13:3482d315877c 9 PwmOut pwmpin(D6);
Peppypeppy 13:3482d315877c 10 PwmOut pwmpin2(D5);
Peppypeppy 13:3482d315877c 11 DigitalOut directionpin2(D4);
Peppypeppy 13:3482d315877c 12 DigitalOut directionpin(D7);
Spekdon 14:c4dbce7de430 13 MODSERIAL pc(USBTX, USBRX);
Spekdon 14:c4dbce7de430 14 //Serial pc(USBTX, USBRX);
MaikOvermars 0:4cb1de41d049 15
Spekdon 14:c4dbce7de430 16 QEI wheel2 (D13, D12, NC, 32, QEI::X2_ENCODING);
Spekdon 14:c4dbce7de430 17 QEI wheel1 (D11, D10, NC, 32, QEI::X2_ENCODING);
Peppypeppy 13:3482d315877c 18 float u1,u2 = 0;
MaikOvermars 0:4cb1de41d049 19
Peppypeppy 13:3482d315877c 20 // for trajectory control:
Peppypeppy 13:3482d315877c 21 double T = 1; // time to get to destination
Peppypeppy 13:3482d315877c 22 double currentx, currenty;
Peppypeppy 13:3482d315877c 23 double currentq1, currentq2;
Spekdon 14:c4dbce7de430 24 // end
Spekdon 14:c4dbce7de430 25 int pulses1, pulses2 = 0;
Spekdon 14:c4dbce7de430 26 float angle1, angle2;
Spekdon 14:c4dbce7de430 27 int realangle1, realangle2;
Spekdon 14:c4dbce7de430 28 double error1;
Spekdon 14:c4dbce7de430 29 double error2;
Spekdon 14:c4dbce7de430 30 double actualoutput1, actualoutput2 = 0;
Spekdon 14:c4dbce7de430 31 bool reached;
MaikOvermars 0:4cb1de41d049 32
MaikOvermars 0:4cb1de41d049 33
Spekdon 14:c4dbce7de430 34 Ticker adjust_ref;
Spekdon 14:c4dbce7de430 35 Ticker pid;
Spekdon 14:c4dbce7de430 36
Peppypeppy 13:3482d315877c 37 float angle_resolution = (360.0/32.0)*(1.0/131.0); //degrees/counts
Spekdon 14:c4dbce7de430 38 double Kp = 0.1;
Spekdon 14:c4dbce7de430 39 double Ki = 0.4;
Spekdon 14:c4dbce7de430 40 double Kd = 4;
Peppypeppy 13:3482d315877c 41 extern double samplingfreq = 1000;
Peppypeppy 13:3482d315877c 42
Spekdon 14:c4dbce7de430 43 double L1 = 0.4388;
Spekdon 14:c4dbce7de430 44 double L2 = 0.4508;
Peppypeppy 13:3482d315877c 45 double x01 = 0.0;
Peppypeppy 13:3482d315877c 46 double y01 = 0.0;
Spekdon 14:c4dbce7de430 47 double ref1, ref2;
Spekdon 14:c4dbce7de430 48 double t = 0.0;
Spekdon 14:c4dbce7de430 49 vector<double> inv_kin_ref;
Spekdon 14:c4dbce7de430 50 vector<double> newconfig;
Spekdon 14:c4dbce7de430 51 vector<double> currentxy;
MaikOvermars 0:4cb1de41d049 52
Spekdon 14:c4dbce7de430 53 double timetogetthere = 0.01;
Spekdon 14:c4dbce7de430 54 double vx_des = 0.05;
Spekdon 14:c4dbce7de430 55 double vy_des = -0.05;
Spekdon 14:c4dbce7de430 56
Spekdon 14:c4dbce7de430 57 float maxpwm = 1;
Spekdon 14:c4dbce7de430 58
Spekdon 14:c4dbce7de430 59 void PID_controller(double error1, double error2, float &u1, float &u2, double actualoutput1, double actualoutput2)
Spekdon 14:c4dbce7de430 60 {
Spekdon 14:c4dbce7de430 61 static double error_prev1 = error1; // initialization with this value only done once!
Spekdon 14:c4dbce7de430 62 static double error_prev2 = error2; // initialization with this value only done once!
Spekdon 14:c4dbce7de430 63 double u_pid2, u_pid1;
Spekdon 14:c4dbce7de430 64 static double error_integral1, error_integral2 = 0;
Spekdon 14:c4dbce7de430 65
Spekdon 14:c4dbce7de430 66 double u_k1 = Kp * error1;
Spekdon 14:c4dbce7de430 67 double u_k2 = Kp * error2;
Spekdon 14:c4dbce7de430 68
Spekdon 14:c4dbce7de430 69 static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Spekdon 14:c4dbce7de430 70 static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Spekdon 14:c4dbce7de430 71
Spekdon 14:c4dbce7de430 72 double error_derivative1 = (error1 - error_prev1)*1/samplingfreq;
Spekdon 14:c4dbce7de430 73 double filtered_error_derivative1 = LowPassFilter1.step(error_derivative1);
Spekdon 14:c4dbce7de430 74 double u_d1 = Kd * filtered_error_derivative1;
Spekdon 14:c4dbce7de430 75
Spekdon 14:c4dbce7de430 76 double error_derivative2 = (error2 - error_prev2)*1/samplingfreq;
Spekdon 14:c4dbce7de430 77 double filtered_error_derivative2 = LowPassFilter2.step(error_derivative2);
Spekdon 14:c4dbce7de430 78 double u_d2 = Kd * filtered_error_derivative2;
Spekdon 14:c4dbce7de430 79
Spekdon 14:c4dbce7de430 80 error_integral1 = error_integral1 +( error1 * 1/samplingfreq);
Spekdon 14:c4dbce7de430 81 error_integral2 = error_integral2 +( error2 * 1/samplingfreq);
Spekdon 14:c4dbce7de430 82 if (error_integral1 > 1){
Spekdon 14:c4dbce7de430 83 error_integral1 = 1;
Spekdon 14:c4dbce7de430 84 }
Spekdon 14:c4dbce7de430 85 else if (error_integral1 < -1){
Spekdon 14:c4dbce7de430 86 error_integral1 = -1;
Spekdon 14:c4dbce7de430 87 }
Spekdon 14:c4dbce7de430 88 if (error_integral2 > 1){
Spekdon 14:c4dbce7de430 89 error_integral2 = 1;
Spekdon 14:c4dbce7de430 90 }
Spekdon 14:c4dbce7de430 91 else if (error_integral2 < -1){
Spekdon 14:c4dbce7de430 92 error_integral2 = -1;
Spekdon 14:c4dbce7de430 93 }
Spekdon 14:c4dbce7de430 94
Spekdon 14:c4dbce7de430 95 double u_i1 = Ki*error_integral1;
Spekdon 14:c4dbce7de430 96 double u_i2 = Ki*error_integral2;
Spekdon 14:c4dbce7de430 97
Spekdon 14:c4dbce7de430 98 u_pid1 = u_k1 + u_d1 + u_i1;
Spekdon 14:c4dbce7de430 99 u_pid2 = u_k2 + u_d2 + u_i2;
Spekdon 14:c4dbce7de430 100
Spekdon 14:c4dbce7de430 101 u1 = -u_pid1;
Spekdon 14:c4dbce7de430 102 u2 = -u_pid2;
Spekdon 14:c4dbce7de430 103
Spekdon 14:c4dbce7de430 104 error_prev1 = error1;
Spekdon 14:c4dbce7de430 105 error_prev2 = error2;
Spekdon 14:c4dbce7de430 106
Spekdon 14:c4dbce7de430 107
Spekdon 14:c4dbce7de430 108 }
Spekdon 14:c4dbce7de430 109
Spekdon 14:c4dbce7de430 110 vector<double> forwardkinematics_function(double& q1, double& q2) {
Peppypeppy 13:3482d315877c 111 // input are joint angles, output are x and y position of end effector
Spekdon 14:c4dbce7de430 112 q1 = (q1 + 90)*(3.14/180);
Spekdon 14:c4dbce7de430 113 q2 = (q2 + 180)*(3.14/180);
Peppypeppy 13:3482d315877c 114
Peppypeppy 13:3482d315877c 115 currentx = x01 + L1*cos(q1)-L2*cos(q2);
Spekdon 14:c4dbce7de430 116 currenty = y01 + L1 * sin(q1) - L2 * sin(q2);
Spekdon 14:c4dbce7de430 117
Spekdon 14:c4dbce7de430 118 vector<double> xy;
Spekdon 14:c4dbce7de430 119 xy.push_back(currentx);
Spekdon 14:c4dbce7de430 120 xy.push_back(currenty);
Spekdon 14:c4dbce7de430 121 return xy;
Spekdon 14:c4dbce7de430 122
MaikOvermars 0:4cb1de41d049 123 }
Peppypeppy 13:3482d315877c 124
Spekdon 14:c4dbce7de430 125 vector<double> inversekinematics_function(const double& T, double& q1, double& q2, double& des_vx, double& des_vy, double ref1, double ref2) {
Spekdon 14:c4dbce7de430 126 // x, y: positions of end effector | T: time to hold current velocity | qref1, qref2: reference thetas | q1, q2: current thetas | vx, vy: desired x, y velocities
Peppypeppy 13:3482d315877c 127
Peppypeppy 13:3482d315877c 128 // pseudo inverse jacobian to get joint speeds
Peppypeppy 13:3482d315877c 129 // input are desired vx and vy of end effector, output joint angle speeds
Peppypeppy 13:3482d315877c 130
Peppypeppy 13:3482d315877c 131 double q1_star_des; // desired joint velocity of q1_star
Peppypeppy 13:3482d315877c 132 double q2_star_des; // same as above but then for q2_star
Spekdon 14:c4dbce7de430 133 double C;
Peppypeppy 13:3482d315877c 134 double qref1, qref2;
Peppypeppy 13:3482d315877c 135
Peppypeppy 13:3482d315877c 136 // The calculation below assumes that the end effector position is calculated before this function is executed
Peppypeppy 13:3482d315877c 137 // In our case the determinant will not equal zero, hence no problems with singularies I think.
Spekdon 14:c4dbce7de430 138 //
Spekdon 14:c4dbce7de430 139 //
Spekdon 14:c4dbce7de430 140
Spekdon 14:c4dbce7de430 141 q1 = (ref1 + 90)*(3.14/180);
Spekdon 14:c4dbce7de430 142 q2 = (ref2 + 180)*(3.14/180);
Spekdon 14:c4dbce7de430 143
Spekdon 14:c4dbce7de430 144 C = L1*L2*sin(q1)*cos(q2) - L1*L2*sin(q2)*cos(q1);
Spekdon 14:c4dbce7de430 145 q1_star_des = double(1/C)*(-L2*cos(q2)*des_vy - L2*sin(q2)*des_vx);
Spekdon 14:c4dbce7de430 146 q2_star_des = double(1/C)*((L2*cos(q2)-L1*cos(q1))*des_vy + (L2*sin(q2)-L1*sin(q1))*des_vx);
Spekdon 14:c4dbce7de430 147
Spekdon 14:c4dbce7de430 148 qref1 = T*q1_star_des;
Spekdon 14:c4dbce7de430 149 qref2 = T*(q2_star_des+q1_star_des);
Spekdon 14:c4dbce7de430 150
Spekdon 14:c4dbce7de430 151 qref1 = qref1*(180/3.14);
Spekdon 14:c4dbce7de430 152 qref2 = qref2*(180/3.14);
Peppypeppy 13:3482d315877c 153
Spekdon 14:c4dbce7de430 154 double testq1 = ref1+qref1;
Spekdon 14:c4dbce7de430 155 double testq2 = ref2+qref2;
Spekdon 14:c4dbce7de430 156 newconfig = forwardkinematics_function(testq1, testq2);
Peppypeppy 13:3482d315877c 157
Spekdon 14:c4dbce7de430 158 if (sqrt(pow(newconfig[0],2)+pow(newconfig[1],2)) > 0.73)
Spekdon 14:c4dbce7de430 159 {
Spekdon 14:c4dbce7de430 160 qref1 = ref1;
Spekdon 14:c4dbce7de430 161 qref2 = ref2;
Spekdon 14:c4dbce7de430 162 }
Spekdon 14:c4dbce7de430 163 else
Spekdon 14:c4dbce7de430 164 {
Spekdon 14:c4dbce7de430 165 qref1 = ref1 + qref1;
Spekdon 14:c4dbce7de430 166 qref2 = ref2 + qref2;
Spekdon 14:c4dbce7de430 167 }
Spekdon 14:c4dbce7de430 168
Peppypeppy 13:3482d315877c 169 vector<double> thetas;
Peppypeppy 13:3482d315877c 170 thetas.push_back(qref1);
Peppypeppy 13:3482d315877c 171 thetas.push_back(qref2);
Peppypeppy 13:3482d315877c 172 return thetas;
MaikOvermars 0:4cb1de41d049 173 }
MaikOvermars 0:4cb1de41d049 174
Spekdon 14:c4dbce7de430 175 void control(){
Spekdon 14:c4dbce7de430 176 // Calculate angle of motor 1
Spekdon 14:c4dbce7de430 177 pulses1 = wheel1.getPulses();
Spekdon 14:c4dbce7de430 178 angle1 = -pulses1*angle_resolution;
Spekdon 14:c4dbce7de430 179 realangle1 = int(angle1) % 360;
Spekdon 14:c4dbce7de430 180
Spekdon 14:c4dbce7de430 181 // Calculate angle of motor 2
Spekdon 14:c4dbce7de430 182 pulses2 = wheel2.getPulses();
Spekdon 14:c4dbce7de430 183 angle2 = pulses2*angle_resolution;
Spekdon 14:c4dbce7de430 184 realangle2 = int(angle2) % 360;
Spekdon 14:c4dbce7de430 185
Spekdon 14:c4dbce7de430 186 // Q1 and Q2 current
Spekdon 14:c4dbce7de430 187 currentq1 = angle1;
Spekdon 14:c4dbce7de430 188 currentq2 = angle2;
Spekdon 14:c4dbce7de430 189
Spekdon 14:c4dbce7de430 190 // modify the current x and y pos
Spekdon 14:c4dbce7de430 191 currentxy = forwardkinematics_function(currentq1, currentq2);
Spekdon 14:c4dbce7de430 192 currentx = currentxy[0];
Spekdon 14:c4dbce7de430 193 currenty = currentxy[1];
Spekdon 14:c4dbce7de430 194
Spekdon 14:c4dbce7de430 195 // calculate the reference position using inverse kinematics:
Spekdon 14:c4dbce7de430 196 inv_kin_ref = inversekinematics_function(timetogetthere, currentq1, currentq2, vx_des, vy_des, ref1, ref2);
Spekdon 14:c4dbce7de430 197 ref1 = inv_kin_ref[0];
Spekdon 14:c4dbce7de430 198 ref2 = inv_kin_ref[1];
Spekdon 14:c4dbce7de430 199
Spekdon 14:c4dbce7de430 200 error1 = ref1 - realangle1;
Spekdon 14:c4dbce7de430 201 error2 = ref2 - realangle2;
Spekdon 14:c4dbce7de430 202
Spekdon 14:c4dbce7de430 203 // control stuff:
Spekdon 14:c4dbce7de430 204 PID_controller(error1, error2, u1, u2, actualoutput1, actualoutput2);
Spekdon 14:c4dbce7de430 205
Spekdon 14:c4dbce7de430 206
Spekdon 14:c4dbce7de430 207 // Check if output is larger than maximum pwm
Spekdon 14:c4dbce7de430 208 if (abs(u1) > maxpwm){
Spekdon 14:c4dbce7de430 209 if (u1 > 0){
Spekdon 14:c4dbce7de430 210 u1 = maxpwm;
Spekdon 14:c4dbce7de430 211 }
Spekdon 14:c4dbce7de430 212 else{
Spekdon 14:c4dbce7de430 213 u1 = -maxpwm;
Spekdon 14:c4dbce7de430 214 }
Spekdon 14:c4dbce7de430 215 }
Spekdon 14:c4dbce7de430 216 if (abs(u2) > maxpwm){
Spekdon 14:c4dbce7de430 217 if (u2 > 0){
Spekdon 14:c4dbce7de430 218 u2 = maxpwm;
Spekdon 14:c4dbce7de430 219 }
Spekdon 14:c4dbce7de430 220 else{
Spekdon 14:c4dbce7de430 221 u2 = -maxpwm;
Spekdon 14:c4dbce7de430 222 }
Spekdon 14:c4dbce7de430 223 }
Spekdon 14:c4dbce7de430 224
Spekdon 14:c4dbce7de430 225 pwmpin = fabs(u1);
Spekdon 14:c4dbce7de430 226 pwmpin2 = fabs(u2);
MaikOvermars 0:4cb1de41d049 227
Spekdon 14:c4dbce7de430 228 // 1/true is negative, 0/false is positive
Spekdon 14:c4dbce7de430 229 directionpin2 = u2 < 0;
Spekdon 14:c4dbce7de430 230 directionpin = u1 < 0;
Spekdon 14:c4dbce7de430 231 }
Spekdon 14:c4dbce7de430 232 void measure()
Spekdon 14:c4dbce7de430 233 {
Peppypeppy 13:3482d315877c 234
MaikOvermars 0:4cb1de41d049 235 }
MaikOvermars 0:4cb1de41d049 236
Peppypeppy 13:3482d315877c 237 int main()
Peppypeppy 13:3482d315877c 238 {
Spekdon 14:c4dbce7de430 239 pc.baud(115200);
Peppypeppy 13:3482d315877c 240 pwmpin.period_us(60);
Spekdon 14:c4dbce7de430 241 pid.attach(&control,0.01);
Spekdon 14:c4dbce7de430 242 ref1 = 0;
Spekdon 14:c4dbce7de430 243 ref2 = 0;
Peppypeppy 13:3482d315877c 244
Peppypeppy 13:3482d315877c 245 while (true) {
Spekdon 14:c4dbce7de430 246 pc.printf("error1: %f, error2: %f, pwm1: %f, pwm2: %f \r \n", error1, error2, u1, u2);
Spekdon 14:c4dbce7de430 247 vector<double> currentthing;
Spekdon 14:c4dbce7de430 248 currentthing = forwardkinematics_function(ref1,ref2);
Spekdon 14:c4dbce7de430 249 double cx = currentthing[0];
Spekdon 14:c4dbce7de430 250 double cy = currentthing[1];
Spekdon 14:c4dbce7de430 251 pc.printf("x: %f, y: %f \r\n",cx,cy);
Peppypeppy 13:3482d315877c 252 }
Peppypeppy 13:3482d315877c 253 }