Group 9 BioRobotics
/
rki_try_out
alleen x-richting, speed rete hoog
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?
User | Revision | Line number | New 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 |