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