Group 9 BioRobotics / Mbed 2 deprecated rki_try_out

alleen x-richting, speed rete hoog

Dependencies:   QEI mbed

main.cpp@2:3f849fd62ebb, 2018-10-30 (annotated)

Committer:
s1725696
Date:
Tue Oct 30 08:57:51 2018 +0000
Revision:
2:3f849fd62ebb
Parent:
1:b9595e136a00
Child:
3:f87769ba4a9b
met y-richting erbij, speed1 is mega groot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1725696 0:83a0d1ae0768 1 #include "mbed.h"
s1725696 0:83a0d1ae0768 2 #include "QEI.h"
s1725696 1:b9595e136a00 3 #include <cmath>
s1725696 0:83a0d1ae0768 4
s1725696 0:83a0d1ae0768 5 #define SERIAL_BAUD 115200
s1725696 0:83a0d1ae0768 6
s1725696 0:83a0d1ae0768 7 Serial pc(USBTX,USBRX);
s1725696 0:83a0d1ae0768 8
s1725696 0:83a0d1ae0768 9 DigitalOut dirpin(D4);
s1725696 0:83a0d1ae0768 10 PwmOut pwmpin(D5);
s1725696 0:83a0d1ae0768 11 AnalogIn pot_1(A1);
s1725696 0:83a0d1ae0768 12 DigitalOut dirpin_2(D7);
s1725696 0:83a0d1ae0768 13 PwmOut pwmpin_2(D6);
s1725696 0:83a0d1ae0768 14 AnalogIn pot_2(A2);
s1725696 0:83a0d1ae0768 15
s1725696 0:83a0d1ae0768 16 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //X4 encoding
s1725696 0:83a0d1ae0768 17 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //X4 encoding
s1725696 0:83a0d1ae0768 18
s1725696 2:3f849fd62ebb 19 // motor1 is motor voor rotatie
s1725696 2:3f849fd62ebb 20 // motor2 is motor voor translatie
s1725696 2:3f849fd62ebb 21
s1725696 1:b9595e136a00 22 double tijd = 0.00006; // sec
s1725696 1:b9595e136a00 23 double delta_t = 17*tijd;
s1725696 0:83a0d1ae0768 24 const double pi = 3.14159265358979323846; // constant pi
s1725696 0:83a0d1ae0768 25 volatile double positie_x;
s1725696 0:83a0d1ae0768 26 volatile double positie_y;
s1725696 2:3f849fd62ebb 27 volatile double delta_y; // verandering in y positie, nodig door x-verandering
s1725696 2:3f849fd62ebb 28 volatile double delta_x; // verandering in x positie, nodig door y-verandering
s1725696 2:3f849fd62ebb 29 volatile double dc2_x; // gewenste counts2 verandering in x-rii
s1725696 2:3f849fd62ebb 30 volatile double dc1_x; // gewenste counts1 verandering in x-rii
s1725696 2:3f849fd62ebb 31 volatile double dc2_y; // gewenste counts2 verandering in y-rii
s1725696 2:3f849fd62ebb 32 volatile double dc1_y; // gewenste counts1 verandering in y-rii
s1725696 0:83a0d1ae0768 33 volatile double A; // matrix a is de positie van de pen tov het midden
s1725696 1:b9595e136a00 34 volatile double counts_per_round = 8400;
s1725696 1:b9595e136a00 35 volatile double gamma = (2*pi)/(25*counts_per_round); // gamma is de afstand afgelegd per count door het grote tandwiel
s1725696 0:83a0d1ae0768 36 volatile double Lpc_x; // positie van de pen tov het midden
s1725696 0:83a0d1ae0768 37 volatile double Lpc_y; // positie van de pen tov het midden
s1725696 0:83a0d1ae0768 38 volatile double theta; // hoekiets
s1725696 1:b9595e136a00 39 const double r = 185; // (mm) straal binnencirkel
s1725696 0:83a0d1ae0768 40 const double alpha = (2*pi)/counts_per_round; // afstand afgelegd door pennenbakje per count
s1725696 0:83a0d1ae0768 41 const double l_beta = 32.5; // afstand (mm) dat de pen altijd bij de toren vandaan is door fysieke limitaties
s1725696 0:83a0d1ae0768 42 volatile double counts1;
s1725696 0:83a0d1ae0768 43 volatile double counts2;
s1725696 0:83a0d1ae0768 44 volatile double speed_desired_motor2; // gewenste snelheid van motor2
s1725696 0:83a0d1ae0768 45 volatile double speed_desired_motor1; // gewenste snelheid van motor1
s1725696 0:83a0d1ae0768 46 volatile double max_speed_motor2;
s1725696 0:83a0d1ae0768 47
s1725696 0:83a0d1ae0768 48 int main()
s1725696 0:83a0d1ae0768 49 {
s1725696 0:83a0d1ae0768 50 pc.baud(115200);
s1725696 0:83a0d1ae0768 51 pc.printf("start\r\n"); // om te kijken of het programma uberhaupt start
s1725696 0:83a0d1ae0768 52 pwmpin.period_us(60);
s1725696 1:b9595e136a00 53 pc.printf("tijd = %f \r\n", tijd);
s1725696 1:b9595e136a00 54 pc.printf("delta_t = %f \r\n",delta_t);
s1725696 0:83a0d1ae0768 55
s1725696 0:83a0d1ae0768 56 while(true){
s1725696 0:83a0d1ae0768 57
s1725696 0:83a0d1ae0768 58 // Potmeter 1
s1725696 0:83a0d1ae0768 59 float out_1 = pot_1 * 2.0f;
s1725696 0:83a0d1ae0768 60 float out_2 = out_1 - 1.0f;
s1725696 0:83a0d1ae0768 61 pc.printf("out_2 = %f ", out_2); // + of - x-richting
s1725696 0:83a0d1ae0768 62
s1725696 0:83a0d1ae0768 63 // Potmeter 2
s1725696 0:83a0d1ae0768 64 float out_3 = pot_2 * 2.0f;
s1725696 0:83a0d1ae0768 65 float out_4 = out_3 - 1.0f;
s1725696 0:83a0d1ae0768 66 pc.printf("out_4 = %f ", out_4); // + of - y-richting
s1725696 0:83a0d1ae0768 67
s1725696 0:83a0d1ae0768 68 // Encoder
s1725696 0:83a0d1ae0768 69 counts1 = Encoder1.getPulses();
s1725696 0:83a0d1ae0768 70 counts2 = Encoder2.getPulses();
s1725696 0:83a0d1ae0768 71 pc.printf("counts1=%i counts2=%i ", counts1,counts2);
s1725696 0:83a0d1ae0768 72
s1725696 0:83a0d1ae0768 73 // verwerking potmeter output tot richtingen motor
s1725696 2:3f849fd62ebb 74 positie_x = out_2*20.0*delta_t; // de max is 20 mm/s
s1725696 2:3f849fd62ebb 75 positie_y = out_4*20.0*delta_t;
s1725696 0:83a0d1ae0768 76 theta = gamma*counts2;
s1725696 0:83a0d1ae0768 77 Lpc_x = (r-(alpha*counts1+l_beta))*sin(theta);
s1725696 0:83a0d1ae0768 78 Lpc_y = (r-(alpha*counts1+l_beta))*cos(theta);
s1725696 0:83a0d1ae0768 79 A = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2));
s1725696 0:83a0d1ae0768 80
s1725696 2:3f849fd62ebb 81 // x-richting
s1725696 2:3f849fd62ebb 82 dc2_x = (asin(positie_x/A)/gamma)-counts2;
s1725696 2:3f849fd62ebb 83 delta_y = A*cos(gamma*dc2_x+theta);
s1725696 2:3f849fd62ebb 84 dc1_x = (r-(delta_y/cos(theta))-l_beta)/alpha;
s1725696 2:3f849fd62ebb 85
s1725696 2:3f849fd62ebb 86 // y-richting
s1725696 2:3f849fd62ebb 87 dc2_y = (acos(delta_y/A)/gamma)-counts2;
s1725696 2:3f849fd62ebb 88 delta_x = A*sin(gamma*dc1_y+theta);
s1725696 2:3f849fd62ebb 89 dc1_y = (r-(delta_x/cos(theta))-l_beta)/alpha;
s1725696 0:83a0d1ae0768 90
s1725696 0:83a0d1ae0768 91 // van delta counts naar pwm
s1725696 2:3f849fd62ebb 92 speed_desired_motor1 = ((dc1_x+dc1_y)*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
s1725696 2:3f849fd62ebb 93 speed_desired_motor2 = ((dc2_x+dc2_y)*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65
s1725696 0:83a0d1ae0768 94 max_speed_motor2 = 46.5; // moet nog berekening in
s1725696 0:83a0d1ae0768 95
s1725696 1:b9595e136a00 96 pc.printf("speed1=%f speed2=%f ", speed_desired_motor1,speed_desired_motor2);
s1725696 0:83a0d1ae0768 97
s1725696 1:b9595e136a00 98 double pwm1 = speed_desired_motor1/(pi); // 20.0 mm/s
s1725696 1:b9595e136a00 99 double pwm2 = (speed_desired_motor2*2)/(max_speed_motor2);
s1725696 0:83a0d1ae0768 100
s1725696 2:3f849fd62ebb 101 pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2);
s1725696 0:83a0d1ae0768 102
s1725696 0:83a0d1ae0768 103 // zeg richtingen aan motor
s1725696 0:83a0d1ae0768 104 dirpin.write(pwm2 < 0);
s1725696 0:83a0d1ae0768 105 pwmpin = fabs (pwm2);
s1725696 0:83a0d1ae0768 106 dirpin_2.write(pwm1 < 0);
s1725696 0:83a0d1ae0768 107 pwmpin_2 = fabs (pwm1);
s1725696 0:83a0d1ae0768 108
s1725696 0:83a0d1ae0768 109 wait(0.01); // aantal keer dat een signaal wordt gecheckt en doorgestuurd
s1725696 0:83a0d1ae0768 110 }
s1725696 0:83a0d1ae0768 111 }
s1725696 0:83a0d1ae0768 112