Group 9 BioRobotics / Mbed 2 deprecated rki_try_out

alleen x-richting, speed rete hoog

Dependencies:   QEI mbed

main.cpp@0:83a0d1ae0768, 2018-10-29 (annotated)

Committer:
s1725696
Date:
Mon Oct 29 15:20:12 2018 +0000
Revision:
0:83a0d1ae0768
Child:
1:b9595e136a00
alleen x-richting, speed rete hoog

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 0:83a0d1ae0768 3 #include "math.h"
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 0:83a0d1ae0768 19 const double period = 60.0*pow(10.0,-6.0); // sec
s1725696 0:83a0d1ae0768 20 const double delta_t = 17.0*period;
s1725696 0:83a0d1ae0768 21 const double pi = 3.14159265358979323846; // constant pi
s1725696 0:83a0d1ae0768 22 volatile double positie_x;
s1725696 0:83a0d1ae0768 23 volatile double positie_y;
s1725696 0:83a0d1ae0768 24 volatile double delta_y; // verandering in y positie
s1725696 0:83a0d1ae0768 25 volatile double dc2; // gewenste counts2 verandering
s1725696 0:83a0d1ae0768 26 volatile double dc1; // gewenste counts1 verandering
s1725696 0:83a0d1ae0768 27 volatile double A; // matrix a is de positie van de pen tov het midden
s1725696 0:83a0d1ae0768 28 volatile double counts_per_round = 8400.0;
s1725696 0:83a0d1ae0768 29 volatile double gamma = (2.0*pi)/(25.0*counts_per_round); // gamma is de afstand afgelegd per count door het grote tandwiel
s1725696 0:83a0d1ae0768 30 volatile double Lpc_x; // positie van de pen tov het midden
s1725696 0:83a0d1ae0768 31 volatile double Lpc_y; // positie van de pen tov het midden
s1725696 0:83a0d1ae0768 32 volatile double theta; // hoekiets
s1725696 0:83a0d1ae0768 33 const double r = 185.0; // (mm) straal binnencirkel
s1725696 0:83a0d1ae0768 34 const double alpha = (2*pi)/counts_per_round; // afstand afgelegd door pennenbakje per count
s1725696 0:83a0d1ae0768 35 const double l_beta = 32.5; // afstand (mm) dat de pen altijd bij de toren vandaan is door fysieke limitaties
s1725696 0:83a0d1ae0768 36 volatile double counts1;
s1725696 0:83a0d1ae0768 37 volatile double counts2;
s1725696 0:83a0d1ae0768 38 volatile double speed_desired_motor2; // gewenste snelheid van motor2
s1725696 0:83a0d1ae0768 39 volatile double speed_desired_motor1; // gewenste snelheid van motor1
s1725696 0:83a0d1ae0768 40 volatile double max_speed_motor2;
s1725696 0:83a0d1ae0768 41
s1725696 0:83a0d1ae0768 42 int main()
s1725696 0:83a0d1ae0768 43 {
s1725696 0:83a0d1ae0768 44 pc.baud(115200);
s1725696 0:83a0d1ae0768 45 pc.printf("start\r\n"); // om te kijken of het programma uberhaupt start
s1725696 0:83a0d1ae0768 46 pwmpin.period_us(60);
s1725696 0:83a0d1ae0768 47
s1725696 0:83a0d1ae0768 48 while(true){
s1725696 0:83a0d1ae0768 49
s1725696 0:83a0d1ae0768 50 // Potmeter 1
s1725696 0:83a0d1ae0768 51 float out_1 = pot_1 * 2.0f;
s1725696 0:83a0d1ae0768 52 float out_2 = out_1 - 1.0f;
s1725696 0:83a0d1ae0768 53 pc.printf("out_2 = %f ", out_2); // + of - x-richting
s1725696 0:83a0d1ae0768 54
s1725696 0:83a0d1ae0768 55 // Potmeter 2
s1725696 0:83a0d1ae0768 56 float out_3 = pot_2 * 2.0f;
s1725696 0:83a0d1ae0768 57 float out_4 = out_3 - 1.0f;
s1725696 0:83a0d1ae0768 58 pc.printf("out_4 = %f ", out_4); // + of - y-richting
s1725696 0:83a0d1ae0768 59
s1725696 0:83a0d1ae0768 60 // Encoder
s1725696 0:83a0d1ae0768 61 counts1 = Encoder1.getPulses();
s1725696 0:83a0d1ae0768 62 counts2 = Encoder2.getPulses();
s1725696 0:83a0d1ae0768 63 pc.printf("counts1=%i counts2=%i ", counts1,counts2);
s1725696 0:83a0d1ae0768 64
s1725696 0:83a0d1ae0768 65 // verwerking potmeter output tot richtingen motor
s1725696 0:83a0d1ae0768 66 positie_x = out_2*delta_t;
s1725696 0:83a0d1ae0768 67 positie_y = out_4*delta_t;
s1725696 0:83a0d1ae0768 68 theta = gamma*counts2;
s1725696 0:83a0d1ae0768 69 Lpc_x = (r-(alpha*counts1+l_beta))*sin(theta);
s1725696 0:83a0d1ae0768 70 Lpc_y = (r-(alpha*counts1+l_beta))*cos(theta);
s1725696 0:83a0d1ae0768 71 A = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2));
s1725696 0:83a0d1ae0768 72
s1725696 0:83a0d1ae0768 73 dc2 = (asin(positie_x/A)/gamma)-counts2;
s1725696 0:83a0d1ae0768 74 delta_y = A*cos(gamma*dc2+theta);
s1725696 0:83a0d1ae0768 75 dc1 = ((r-delta_y)/cos(theta)-l_beta)/alpha;
s1725696 0:83a0d1ae0768 76
s1725696 0:83a0d1ae0768 77 // van delta counts naar pwm
s1725696 0:83a0d1ae0768 78 speed_desired_motor1 = (dc1*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi
s1725696 0:83a0d1ae0768 79 speed_desired_motor2 = (dc2*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65
s1725696 0:83a0d1ae0768 80 max_speed_motor2 = 46.5; // moet nog berekening in
s1725696 0:83a0d1ae0768 81
s1725696 0:83a0d1ae0768 82 pc.printf("speed1=%d speed2=%d ", speed_desired_motor1,speed_desired_motor2);
s1725696 0:83a0d1ae0768 83
s1725696 0:83a0d1ae0768 84 double pwm1 = speed_desired_motor1/(20.0*pi); // 20.0 mm/s
s1725696 0:83a0d1ae0768 85 double pwm2 = (speed_desired_motor2*2.0)/(20.0*max_speed_motor2);
s1725696 0:83a0d1ae0768 86
s1725696 0:83a0d1ae0768 87 pc.printf("pwm1=%d pwm2=%d\r\n",pwm1,pwm2);
s1725696 0:83a0d1ae0768 88
s1725696 0:83a0d1ae0768 89 // zeg richtingen aan motor
s1725696 0:83a0d1ae0768 90 dirpin.write(pwm2 < 0);
s1725696 0:83a0d1ae0768 91 pwmpin = fabs (pwm2);
s1725696 0:83a0d1ae0768 92 dirpin_2.write(pwm1 < 0);
s1725696 0:83a0d1ae0768 93 pwmpin_2 = fabs (pwm1);
s1725696 0:83a0d1ae0768 94
s1725696 0:83a0d1ae0768 95
s1725696 0:83a0d1ae0768 96 wait(0.01); // aantal keer dat een signaal wordt gecheckt en doorgestuurd
s1725696 0:83a0d1ae0768 97 }
s1725696 0:83a0d1ae0768 98 }
s1725696 0:83a0d1ae0768 99