Sander Geurts / Mbed 2 deprecated DEMOmode

Combination of working libraries useful for the BioRobotics course.

Dependencies:   mbed QEI HIDScope BiQuad4th_order biquadFilter MODSERIAL FastPWM

main.cpp@3:695daa59763d, 2019-10-24 (annotated)

Committer:
sanou8
Date:
Thu Oct 24 12:12:58 2019 +0000
Revision:
3:695daa59763d
Parent:
2:3feeeb434275 
DEMO apparaat

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
sanou8 3:695daa59763d 2 #include "MODSERIAL.h"
sanou8 2:3feeeb434275 3 #include "QEI.h"
sanou8 3:695daa59763d 4 //#include "FilterDesign.h"
sanou8 3:695daa59763d 5 #include "BiQuad.h"
sanou8 3:695daa59763d 6 #include "BiQuad4.h"
sanou8 3:695daa59763d 7 #include "HIDScope.h"
sanou8 3:695daa59763d 8
sanou8 3:695daa59763d 9
sanou8 3:695daa59763d 10 QEI Encoder1(D12,D13,NC,32);
sanou8 3:695daa59763d 11 QEI Encoder2(D10,D11,NC,32);
sanou8 3:695daa59763d 12
sanou8 3:695daa59763d 13 DigitalOut M1(D4);
sanou8 3:695daa59763d 14 DigitalOut M2(D7);
sanou8 3:695daa59763d 15 DigitalIn button(SW3);
sanou8 3:695daa59763d 16
sanou8 3:695daa59763d 17 PwmOut E1(D5);
sanou8 3:695daa59763d 18 PwmOut E2(D6);
sanou8 3:695daa59763d 19
sanou8 3:695daa59763d 20
sanou8 3:695daa59763d 21 // GLOBAL VALUES
sanou8 3:695daa59763d 22 float pi = 3.14159265359;
sanou8 3:695daa59763d 23 int counts1; // Counts of encoder 1
sanou8 3:695daa59763d 24 int counts2; // Counts of encoder 2
sanou8 3:695daa59763d 25 float theta1; // Angle of motor 1 (rad)
sanou8 3:695daa59763d 26 float theta2; // Angle of motor 2 (rad)
sanou8 3:695daa59763d 27 float vel_1; // Velocity of motor 1
sanou8 3:695daa59763d 28 float vel_2; // Velocity of motor 2
sanou8 3:695daa59763d 29 float theta1_prev = 0.0; // Previous angles set to zero
sanou8 3:695daa59763d 30 float theta2_prev = 0.0;
sanou8 3:695daa59763d 31 double dirx = 1.0; //determine the direction of the setpoint placement
sanou8 3:695daa59763d 32 double diry = 1.0; //determine the direction of the setpoint placement
sanou8 3:695daa59763d 33 volatile double U1; // Motor voltage for motor 1
sanou8 3:695daa59763d 34 volatile double U2; // Motor voltage for motor 2
sanou8 3:695daa59763d 35 float tijd = 0.005;
RobertoO 0:67c50348f842 36
RobertoO 0:67c50348f842 37
sanou8 3:695daa59763d 38 // Inverse kinematics
sanou8 3:695daa59763d 39 const double La = 200.0; // length of the first link
sanou8 3:695daa59763d 40 const double Lb = 200.0; // length of the second link
sanou8 3:695daa59763d 41 volatile double q1_diff;
sanou8 3:695daa59763d 42 volatile double q2_diff;
sanou8 3:695daa59763d 43 const double sq = 2.0; // to square numbers
sanou8 3:695daa59763d 44
sanou8 3:695daa59763d 45
sanou8 3:695daa59763d 46 // DEMO
sanou8 3:695daa59763d 47 double point1x = 0; // Coordinates of first prescribed point
sanou8 3:695daa59763d 48 double point1y = 0;
sanou8 3:695daa59763d 49 double point2x = 0; // Coordinates of second prescribed point
sanou8 3:695daa59763d 50 double point2y = 80;
sanou8 3:695daa59763d 51 double point3x = -50; // Coordinates of third prescribed point
sanou8 3:695daa59763d 52 double point3y = 80;
sanou8 3:695daa59763d 53 double point4x = -50; // Coordinates of fourth prescribed point
sanou8 3:695daa59763d 54 double point4y = 30;
sanou8 3:695daa59763d 55 double point5x = 0; // Coordinates of fifth prescribed point
sanou8 3:695daa59763d 56 double point5y = 30;
sanou8 3:695daa59763d 57 volatile int track = 1; // Start with the track from zero to first prescribed point
sanou8 2:3feeeb434275 58
sanou8 3:695daa59763d 59 const double stepsize1 = 0.15;
sanou8 3:695daa59763d 60 const double stepsize2 = 0.25;
sanou8 3:695daa59763d 61 const double setpoint_error = 0.3;
sanou8 3:695daa59763d 62 double setpointx = 0 ;
sanou8 3:695daa59763d 63 double setpointy = 0 ;
sanou8 3:695daa59763d 64
sanou8 3:695daa59763d 65 void ReadEncoder1() ;
sanou8 3:695daa59763d 66 void ReadEncoder2() ;
sanou8 3:695daa59763d 67 double counts2angle1() ;
sanou8 3:695daa59763d 68 double counts2angle2() ;
sanou8 3:695daa59763d 69 void ChangeDirectionX();
sanou8 3:695daa59763d 70 void ChangeDirectionY() ;
sanou8 3:695daa59763d 71 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);
sanou8 3:695daa59763d 72 double determinedemosetx(double setpointx, double setpointy) ;
sanou8 3:695daa59763d 73 double determinedemosety(double setpointx, double setpointy) ;
sanou8 3:695daa59763d 74 double calcRot1(double xDes, double yDes) ;
sanou8 3:695daa59763d 75 double calcRot2(double xDes, double yDes) ;
sanou8 3:695daa59763d 76
sanou8 3:695daa59763d 77
sanou8 3:695daa59763d 78
sanou8 3:695daa59763d 79
sanou8 3:695daa59763d 80 // ----------------------------------------------
sanou8 3:695daa59763d 81 // ------- MAIN FUNCTION ------------------------
sanou8 3:695daa59763d 82 // ----------------------------------------------
RobertoO 0:67c50348f842 83
RobertoO 0:67c50348f842 84 int main()
RobertoO 0:67c50348f842 85 {
sanou8 3:695daa59763d 86
sanou8 2:3feeeb434275 87 while (true)
sanou8 2:3feeeb434275 88 {
sanou8 3:695daa59763d 89 moveMotorTo(&M1,&E1,&Encoder1,calcRot1(setpointx,setpointy)) ;
sanou8 3:695daa59763d 90 moveMotorTo(&M2,&E2,&Encoder2,calcRot2(setpointx,setpointy)) ;
RobertoO 0:67c50348f842 91 }
sanou8 3:695daa59763d 92 }
sanou8 3:695daa59763d 93
sanou8 3:695daa59763d 94
sanou8 3:695daa59763d 95 // Encoders
sanou8 3:695daa59763d 96 void ReadEncoder1() // Read Encoder of motor 1.
sanou8 3:695daa59763d 97 {
sanou8 3:695daa59763d 98 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
sanou8 3:695daa59763d 99 theta1 = (float(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
sanou8 3:695daa59763d 100 vel_1 = (theta1 - theta1_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
sanou8 3:695daa59763d 101 theta1_prev = theta1; // Define theta_prev
sanou8 3:695daa59763d 102 }
sanou8 3:695daa59763d 103
sanou8 3:695daa59763d 104 void ReadEncoder2() // Read encoder of motor 2.
sanou8 3:695daa59763d 105 {
sanou8 3:695daa59763d 106 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
sanou8 3:695daa59763d 107 theta2 = (float(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
sanou8 3:695daa59763d 108 vel_2 = (theta2 - theta2_prev) / tijd; // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
sanou8 3:695daa59763d 109 theta2_prev = theta2; // Define theta_prev
sanou8 3:695daa59763d 110 }
sanou8 3:695daa59763d 111 double counts2angle1()
sanou8 3:695daa59763d 112 {
sanou8 3:695daa59763d 113 counts1 = Encoder1.getPulses(); // Counts of outputshaft of motor 1
sanou8 3:695daa59763d 114 theta1 = -(double(counts1)/4200) * 2*pi; // Angle of outputshaft of motor 1
sanou8 3:695daa59763d 115 return theta1;
sanou8 3:695daa59763d 116 }
sanou8 3:695daa59763d 117
sanou8 3:695daa59763d 118 double counts2angle2()
sanou8 3:695daa59763d 119 {
sanou8 3:695daa59763d 120 counts2 = Encoder2.getPulses(); // Counts of outputshaft of motor 2
sanou8 3:695daa59763d 121 theta2 = (double(counts2)/4200) * 2*pi; // Angle of outputshaft of motor 2
sanou8 3:695daa59763d 122 return theta2;
sanou8 3:695daa59763d 123 }
sanou8 3:695daa59763d 124
sanou8 3:695daa59763d 125
sanou8 3:695daa59763d 126
sanou8 3:695daa59763d 127
sanou8 3:695daa59763d 128 //function to change the moving direction of the setpoint
sanou8 3:695daa59763d 129 void ChangeDirectionX(){
sanou8 3:695daa59763d 130 dirx = -1*dirx;
sanou8 3:695daa59763d 131 }
sanou8 3:695daa59763d 132
sanou8 3:695daa59763d 133 void ChangeDirectionY(){
sanou8 3:695daa59763d 134 diry = -1*diry;
sanou8 3:695daa59763d 135 }
sanou8 3:695daa59763d 136
sanou8 3:695daa59763d 137 // DEMO set
sanou8 3:695daa59763d 138
sanou8 3:695daa59763d 139 double determinedemosetx(double setpointx, double setpointy)
sanou8 3:695daa59763d 140 {
sanou8 3:695daa59763d 141
sanou8 3:695daa59763d 142 if (setpointx < point1x && track == 1){
sanou8 3:695daa59763d 143 setpointx = setpointx + stepsize1;
sanou8 3:695daa59763d 144 }
sanou8 3:695daa59763d 145
sanou8 3:695daa59763d 146 // From point 1 to 2
sanou8 3:695daa59763d 147 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)) {
sanou8 3:695daa59763d 148 track = 12;
sanou8 3:695daa59763d 149 }
sanou8 3:695daa59763d 150
sanou8 3:695daa59763d 151 if (setpointx < point2x && track == 12){
sanou8 3:695daa59763d 152 setpointx = point2x;
sanou8 3:695daa59763d 153 }
sanou8 3:695daa59763d 154
sanou8 3:695daa59763d 155 // From point 2 to 3
sanou8 3:695daa59763d 156 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && track == 12){
sanou8 3:695daa59763d 157 track = 23;
sanou8 3:695daa59763d 158 }
sanou8 3:695daa59763d 159
sanou8 3:695daa59763d 160 if (setpointy > point3y && track == 23){
sanou8 3:695daa59763d 161 setpointx = setpointx + stepsize1;
sanou8 3:695daa59763d 162 // Stay on the same y value from point 2 to 3
sanou8 3:695daa59763d 163 }
sanou8 3:695daa59763d 164
sanou8 3:695daa59763d 165 // From point 3 to 4
sanou8 3:695daa59763d 166 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)) {
sanou8 3:695daa59763d 167 setpointy = point4y;
sanou8 3:695daa59763d 168 track = 34;
sanou8 3:695daa59763d 169 }
sanou8 3:695daa59763d 170
sanou8 3:695daa59763d 171 if (setpointx > point4x && track == 34){
sanou8 3:695daa59763d 172 setpointx = setpointx - stepsize2;
sanou8 3:695daa59763d 173 }
sanou8 3:695daa59763d 174
sanou8 3:695daa59763d 175 // From point 4 to 5
sanou8 3:695daa59763d 176 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
sanou8 3:695daa59763d 177 setpointx = point4x;
sanou8 3:695daa59763d 178 track = 45;
sanou8 3:695daa59763d 179 }
sanou8 3:695daa59763d 180
sanou8 3:695daa59763d 181
sanou8 3:695daa59763d 182 if (setpointy < point5y && track == 45){
sanou8 3:695daa59763d 183 setpointx = point5x;
sanou8 3:695daa59763d 184 // From point 4 to 5, stay on the same x value
sanou8 3:695daa59763d 185 }
sanou8 3:695daa59763d 186
sanou8 3:695daa59763d 187 // From point 5 to 1
sanou8 3:695daa59763d 188 if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){
sanou8 3:695daa59763d 189 setpointx = point5x;
sanou8 3:695daa59763d 190 track = 51;
sanou8 3:695daa59763d 191 }
sanou8 3:695daa59763d 192
sanou8 3:695daa59763d 193
sanou8 3:695daa59763d 194 if (setpointy < point1y && track == 51){
sanou8 3:695daa59763d 195 setpointx = point1x;
sanou8 3:695daa59763d 196 // From point 4 to 5, stay on the same x value
sanou8 3:695daa59763d 197 }
sanou8 3:695daa59763d 198 return setpointx;
sanou8 3:695daa59763d 199 }
sanou8 3:695daa59763d 200
sanou8 3:695daa59763d 201 double determinedemosety(double setpointx, double setpointy)
sanou8 3:695daa59763d 202 {
sanou8 3:695daa59763d 203 // From reference to point 1
sanou8 3:695daa59763d 204 if(setpointy < point1y && track == 1){
sanou8 3:695daa59763d 205 setpointy = setpointy + (stepsize2);
sanou8 3:695daa59763d 206 }
sanou8 3:695daa59763d 207
sanou8 3:695daa59763d 208 // From point 1 to 2
sanou8 3:695daa59763d 209 if (fabs(setpointx - point1x) <= setpoint_error && fabs(setpointy - point1y) <= setpoint_error && (track == 1 || track == 51)){
sanou8 3:695daa59763d 210 setpointy = point2y;
sanou8 3:695daa59763d 211 // Stay on the same y from point 1 to 2.
sanou8 3:695daa59763d 212 track = 12;
sanou8 3:695daa59763d 213 }
sanou8 3:695daa59763d 214 if (setpointx < point2x && track == 12){
sanou8 3:695daa59763d 215 setpointy = point2y;
sanou8 3:695daa59763d 216 }
sanou8 3:695daa59763d 217
sanou8 3:695daa59763d 218 // From point 2 to 3
sanou8 3:695daa59763d 219 if (fabs(setpointx - point2x) <= setpoint_error && fabs(setpointy - point2y) <= setpoint_error && (track == 12)){
sanou8 3:695daa59763d 220 setpointx = point3x;
sanou8 3:695daa59763d 221 track = 23;
sanou8 3:695daa59763d 222 }
sanou8 3:695daa59763d 223 if ((setpointy > point3y) && (track == 23)){
sanou8 3:695daa59763d 224 setpointy = setpointy + (-stepsize2);
sanou8 3:695daa59763d 225 track = 23;
sanou8 3:695daa59763d 226 }
sanou8 3:695daa59763d 227
sanou8 3:695daa59763d 228 // From point 3 to 4
sanou8 3:695daa59763d 229 if ((fabs(setpointx - point3x) <= setpoint_error) && (fabs(setpointy - point3y) <= setpoint_error) && (track == 23)){
sanou8 3:695daa59763d 230 setpointy = setpointy;
sanou8 3:695daa59763d 231 track = 34;
sanou8 3:695daa59763d 232 }
sanou8 3:695daa59763d 233 if (setpointx > point4x && track == 34){
sanou8 3:695daa59763d 234 setpointy = setpointy;
sanou8 3:695daa59763d 235 }
sanou8 3:695daa59763d 236
sanou8 3:695daa59763d 237 // From point 4 to 5
sanou8 3:695daa59763d 238 if ((fabs(setpointx - point4x) <= setpoint_error) && (fabs(setpointy - point4y) <= setpoint_error) && (track == 34)){
sanou8 3:695daa59763d 239 track = 45;
sanou8 3:695daa59763d 240 }
sanou8 3:695daa59763d 241
sanou8 3:695daa59763d 242 if (setpointy > point5y && track == 45){
sanou8 3:695daa59763d 243 setpointy = setpointy - (stepsize2);
sanou8 3:695daa59763d 244 // From point 4 to 5, stay on the same x value.
sanou8 3:695daa59763d 245 }
sanou8 3:695daa59763d 246 // From point 5 to 1
sanou8 3:695daa59763d 247 if ((fabs(setpointx - point5x) <= setpoint_error) && (fabs(setpointy - point5y) <= setpoint_error) && (track == 45)){
sanou8 3:695daa59763d 248 track = 51;
sanou8 3:695daa59763d 249 }
sanou8 3:695daa59763d 250
sanou8 3:695daa59763d 251 if (setpointy < point1y && track == 51){
sanou8 3:695daa59763d 252 setpointy = setpointy + (stepsize2);
sanou8 3:695daa59763d 253 // From point 4 to 5, stay on the same x value.
sanou8 3:695daa59763d 254 }
sanou8 3:695daa59763d 255 return setpointy;
sanou8 3:695daa59763d 256
sanou8 3:695daa59763d 257 }
sanou8 3:695daa59763d 258
sanou8 3:695daa59763d 259 //function to mave a motor to a certain number of rotations, counted from the start of the program.
sanou8 3:695daa59763d 260 //parameters:
sanou8 3:695daa59763d 261 //DigitalOut *M = pointer to direction cpntol pin of motor
sanou8 3:695daa59763d 262 //PwmOut *E = pointer to speed contorl pin of motor
sanou8 3:695daa59763d 263 //QEI *Enc = pointer to encoder of motor
sanou8 3:695daa59763d 264 //float rotDes = desired rotation
sanou8 3:695daa59763d 265 void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
sanou8 3:695daa59763d 266 {
sanou8 3:695daa59763d 267 float pErrorC;
sanou8 3:695daa59763d 268 float pErrorP = 0;
sanou8 3:695daa59763d 269 float iError = 0;
sanou8 3:695daa59763d 270 float dError;
sanou8 3:695daa59763d 271
sanou8 3:695daa59763d 272 float Kp = 5;
sanou8 3:695daa59763d 273 float Ki = 0.01;
sanou8 3:695daa59763d 274 float Kd = 0.7;
sanou8 3:695daa59763d 275
sanou8 3:695daa59763d 276 float rotC = Enc->getPulses()/(32*131.25);
sanou8 3:695daa59763d 277 float rotP = 0;
sanou8 3:695daa59763d 278 float MotorPWM;
sanou8 3:695daa59763d 279
sanou8 3:695daa59763d 280 Timer t;
sanou8 3:695daa59763d 281 float tieme = 0;
sanou8 3:695daa59763d 282
sanou8 3:695daa59763d 283 t.start();
sanou8 3:695daa59763d 284 do {
sanou8 3:695daa59763d 285 pErrorP = pErrorC;
sanou8 3:695daa59763d 286 pErrorC = rotDes - rotC;
sanou8 3:695daa59763d 287 iError = iError + pErrorC*tieme;
sanou8 3:695daa59763d 288 dError = (pErrorC - pErrorP)/tieme;
sanou8 3:695daa59763d 289
sanou8 3:695daa59763d 290 MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;
sanou8 3:695daa59763d 291
sanou8 3:695daa59763d 292 if(MotorPWM > 0) {
sanou8 3:695daa59763d 293 *M = 0;
sanou8 3:695daa59763d 294 *E = MotorPWM;
sanou8 3:695daa59763d 295 } else {
sanou8 3:695daa59763d 296 *M = 1;
sanou8 3:695daa59763d 297 *E = -MotorPWM;
sanou8 3:695daa59763d 298 }
sanou8 3:695daa59763d 299
sanou8 3:695daa59763d 300 rotP = rotC;
sanou8 3:695daa59763d 301 rotC = Enc->getPulses()/(32*131.25);
sanou8 3:695daa59763d 302 tieme = t.read();
sanou8 3:695daa59763d 303 t.reset();
sanou8 3:695daa59763d 304 } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
sanou8 3:695daa59763d 305 t.stop();
sanou8 3:695daa59763d 306 }
sanou8 3:695daa59763d 307
sanou8 3:695daa59763d 308 //double that calculates the rotation of one arm.
sanou8 3:695daa59763d 309 //parameters:
sanou8 3:695daa59763d 310 //double xDes = ofset of the arm's shaft in cm in the x direction
sanou8 3:695daa59763d 311 //double yDes = ofset of the arm's shaft in cm in the y direction
sanou8 3:695daa59763d 312 double calcRot1(double xDes, double yDes)
sanou8 3:695daa59763d 313 {
sanou8 3:695daa59763d 314 return 6*((atan(yDes/xDes) - 0.5*(pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*pi));
sanou8 3:695daa59763d 315 };
sanou8 3:695daa59763d 316
sanou8 3:695daa59763d 317 //double that calculates the rotation of the other arm.
sanou8 3:695daa59763d 318 //parameters:
sanou8 3:695daa59763d 319 //double xDes = ofset of the arm's shaft in cm in the x direction
sanou8 3:695daa59763d 320 //double yDes = ofset of the arm's shaft in cm in the y direction
sanou8 3:695daa59763d 321 double calcRot2(double xDes, double yDes)
sanou8 3:695daa59763d 322 {
sanou8 3:695daa59763d 323 return 6*((atan(yDes/xDes) + 0.5*(pi - acos((pow(xDes, 2.0) + pow(yDes, 2.0) - 2*pow(20.0, 2.0))/(-2*pow(20.0, 2.0)))))/(2*pi));
sanou8 3:695daa59763d 324 };