Group 9 BioRobotics
/
rki_try_out
alleen x-richting, speed rete hoog
main.cpp@1:b9595e136a00, 2018-10-29 (annotated)
- Committer:
- s1725696
- Date:
- Mon Oct 29 16:07:09 2018 +0000
- Revision:
- 1:b9595e136a00
- Parent:
- 0:83a0d1ae0768
- Child:
- 2:3f849fd62ebb
pwm is mega klein rest werkt
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 | 1:b9595e136a00 | 19 | double tijd = 0.00006; // sec |
s1725696 | 1:b9595e136a00 | 20 | double delta_t = 17*tijd; |
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 | 1:b9595e136a00 | 28 | volatile double counts_per_round = 8400; |
s1725696 | 1:b9595e136a00 | 29 | volatile double gamma = (2*pi)/(25*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 | 1:b9595e136a00 | 33 | const double r = 185; // (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 | 1:b9595e136a00 | 47 | pc.printf("tijd = %f \r\n", tijd); |
s1725696 | 1:b9595e136a00 | 48 | pc.printf("delta_t = %f \r\n",delta_t); |
s1725696 | 0:83a0d1ae0768 | 49 | |
s1725696 | 0:83a0d1ae0768 | 50 | while(true){ |
s1725696 | 0:83a0d1ae0768 | 51 | |
s1725696 | 0:83a0d1ae0768 | 52 | // Potmeter 1 |
s1725696 | 0:83a0d1ae0768 | 53 | float out_1 = pot_1 * 2.0f; |
s1725696 | 0:83a0d1ae0768 | 54 | float out_2 = out_1 - 1.0f; |
s1725696 | 0:83a0d1ae0768 | 55 | pc.printf("out_2 = %f ", out_2); // + of - x-richting |
s1725696 | 0:83a0d1ae0768 | 56 | |
s1725696 | 0:83a0d1ae0768 | 57 | // Potmeter 2 |
s1725696 | 0:83a0d1ae0768 | 58 | float out_3 = pot_2 * 2.0f; |
s1725696 | 0:83a0d1ae0768 | 59 | float out_4 = out_3 - 1.0f; |
s1725696 | 0:83a0d1ae0768 | 60 | pc.printf("out_4 = %f ", out_4); // + of - y-richting |
s1725696 | 0:83a0d1ae0768 | 61 | |
s1725696 | 0:83a0d1ae0768 | 62 | // Encoder |
s1725696 | 0:83a0d1ae0768 | 63 | counts1 = Encoder1.getPulses(); |
s1725696 | 0:83a0d1ae0768 | 64 | counts2 = Encoder2.getPulses(); |
s1725696 | 0:83a0d1ae0768 | 65 | pc.printf("counts1=%i counts2=%i ", counts1,counts2); |
s1725696 | 0:83a0d1ae0768 | 66 | |
s1725696 | 0:83a0d1ae0768 | 67 | // verwerking potmeter output tot richtingen motor |
s1725696 | 0:83a0d1ae0768 | 68 | positie_x = out_2*delta_t; |
s1725696 | 0:83a0d1ae0768 | 69 | positie_y = out_4*delta_t; |
s1725696 | 0:83a0d1ae0768 | 70 | theta = gamma*counts2; |
s1725696 | 0:83a0d1ae0768 | 71 | Lpc_x = (r-(alpha*counts1+l_beta))*sin(theta); |
s1725696 | 0:83a0d1ae0768 | 72 | Lpc_y = (r-(alpha*counts1+l_beta))*cos(theta); |
s1725696 | 0:83a0d1ae0768 | 73 | A = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2)); |
s1725696 | 0:83a0d1ae0768 | 74 | |
s1725696 | 0:83a0d1ae0768 | 75 | dc2 = (asin(positie_x/A)/gamma)-counts2; |
s1725696 | 0:83a0d1ae0768 | 76 | delta_y = A*cos(gamma*dc2+theta); |
s1725696 | 0:83a0d1ae0768 | 77 | dc1 = ((r-delta_y)/cos(theta)-l_beta)/alpha; |
s1725696 | 0:83a0d1ae0768 | 78 | |
s1725696 | 0:83a0d1ae0768 | 79 | // van delta counts naar pwm |
s1725696 | 0:83a0d1ae0768 | 80 | speed_desired_motor1 = (dc1*alpha)/delta_t; // voor max 2cm/s translatie -> range pwm: -1/pi - 1/pi |
s1725696 | 0:83a0d1ae0768 | 81 | speed_desired_motor2 = (dc2*gamma)/delta_t; // voor max rotatie -> range pwm: -2/4,65 - 2/4,65 |
s1725696 | 0:83a0d1ae0768 | 82 | max_speed_motor2 = 46.5; // moet nog berekening in |
s1725696 | 0:83a0d1ae0768 | 83 | |
s1725696 | 1:b9595e136a00 | 84 | pc.printf("speed1=%f speed2=%f ", speed_desired_motor1,speed_desired_motor2); |
s1725696 | 0:83a0d1ae0768 | 85 | |
s1725696 | 1:b9595e136a00 | 86 | double pwm1 = speed_desired_motor1/(pi); // 20.0 mm/s |
s1725696 | 1:b9595e136a00 | 87 | double pwm2 = (speed_desired_motor2*2)/(max_speed_motor2); |
s1725696 | 0:83a0d1ae0768 | 88 | |
s1725696 | 1:b9595e136a00 | 89 | pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2); |
s1725696 | 0:83a0d1ae0768 | 90 | |
s1725696 | 0:83a0d1ae0768 | 91 | // zeg richtingen aan motor |
s1725696 | 0:83a0d1ae0768 | 92 | dirpin.write(pwm2 < 0); |
s1725696 | 0:83a0d1ae0768 | 93 | pwmpin = fabs (pwm2); |
s1725696 | 0:83a0d1ae0768 | 94 | dirpin_2.write(pwm1 < 0); |
s1725696 | 0:83a0d1ae0768 | 95 | pwmpin_2 = fabs (pwm1); |
s1725696 | 0:83a0d1ae0768 | 96 | |
s1725696 | 0:83a0d1ae0768 | 97 | |
s1725696 | 0:83a0d1ae0768 | 98 | wait(0.01); // aantal keer dat een signaal wordt gecheckt en doorgestuurd |
s1725696 | 0:83a0d1ae0768 | 99 | } |
s1725696 | 0:83a0d1ae0768 | 100 | } |
s1725696 | 0:83a0d1ae0768 | 101 |