Group 9 BioRobotics
/
rki_try_out
alleen x-richting, speed rete hoog
main.cpp@3:f87769ba4a9b, 2018-10-30 (annotated)
- Committer:
- s1725696
- Date:
- Tue Oct 30 11:40:43 2018 +0000
- Revision:
- 3:f87769ba4a9b
- Parent:
- 2:3f849fd62ebb
all put in functions, pwm is still too high
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 | 3:f87769ba4a9b | 19 | // motor1 is motor voor translatie |
s1725696 | 3:f87769ba4a9b | 20 | // motor2 is motor voor rotatie |
s1725696 | 2:3f849fd62ebb | 21 | |
s1725696 | 3:f87769ba4a9b | 22 | double pwmperiod = 0.00006; // period of pwm, in seconds |
s1725696 | 3:f87769ba4a9b | 23 | double delta_t = 17*pwmperiod; // time to make calculations |
s1725696 | 0:83a0d1ae0768 | 24 | const double pi = 3.14159265358979323846; // constant pi |
s1725696 | 3:f87769ba4a9b | 25 | volatile double positie_x; // desired change in x-direction, given by EMG or potmeter |
s1725696 | 3:f87769ba4a9b | 26 | volatile double positie_y; // desired change in y-direction, given by EMG or potmeter |
s1725696 | 3:f87769ba4a9b | 27 | volatile double delta_y; // change in y direction, due to change in x direction |
s1725696 | 3:f87769ba4a9b | 28 | volatile double delta_x; // change in x direction, due to change in y direction |
s1725696 | 3:f87769ba4a9b | 29 | volatile double dc2_x; // desired change in counts2 for x-direction |
s1725696 | 3:f87769ba4a9b | 30 | volatile double dc1_x; // desired change in counts1 for x-direction |
s1725696 | 3:f87769ba4a9b | 31 | volatile double dc2_y; // desired change in counts2 for y-direction |
s1725696 | 3:f87769ba4a9b | 32 | volatile double dc1_y; // desired change in counts1 for y-direction |
s1725696 | 3:f87769ba4a9b | 33 | volatile double A; // position of the pen with respect to the middle |
s1725696 | 1:b9595e136a00 | 34 | volatile double counts_per_round = 8400; |
s1725696 | 3:f87769ba4a9b | 35 | volatile double gamma = (2*pi)/(25*counts_per_round); // distance traveled per count by big gear |
s1725696 | 3:f87769ba4a9b | 36 | volatile double Lpc_x; // position in x-direction of pen with respect to the middle |
s1725696 | 3:f87769ba4a9b | 37 | volatile double Lpc_y; // position in x-direction of pen with respect to the middle |
s1725696 | 3:f87769ba4a9b | 38 | volatile double theta; // total angle traveled |
s1725696 | 3:f87769ba4a9b | 39 | const double r = 185; // (mm) radius inner circle |
s1725696 | 3:f87769ba4a9b | 40 | const double alpha = (2*pi)/counts_per_round; // distance traveled by penholder per count |
s1725696 | 3:f87769ba4a9b | 41 | const double l_beta = 32.5; // distance (mm) that the pen is away from the tower, due to limitations |
s1725696 | 0:83a0d1ae0768 | 42 | volatile double counts1; |
s1725696 | 0:83a0d1ae0768 | 43 | volatile double counts2; |
s1725696 | 3:f87769ba4a9b | 44 | volatile double speed_desired_motor2; // desired speed of motor2 |
s1725696 | 3:f87769ba4a9b | 45 | volatile double speed_desired_motor1; // desired speed of motor1 |
s1725696 | 3:f87769ba4a9b | 46 | volatile double max_speed_motor2; // |
s1725696 | 3:f87769ba4a9b | 47 | |
s1725696 | 0:83a0d1ae0768 | 48 | |
s1725696 | 3:f87769ba4a9b | 49 | // functions |
s1725696 | 3:f87769ba4a9b | 50 | // ----------------------------------------------------------------------------- |
s1725696 | 3:f87769ba4a9b | 51 | void potmeter(float& a, float& b) |
s1725696 | 3:f87769ba4a9b | 52 | { |
s1725696 | 0:83a0d1ae0768 | 53 | // Potmeter 1 |
s1725696 | 3:f87769ba4a9b | 54 | float out_1 = a; |
s1725696 | 3:f87769ba4a9b | 55 | a = (out_1*2.0f) - 1.0f; |
s1725696 | 3:f87769ba4a9b | 56 | pc.printf("out_2 = %f ", a); // + or - x-direction |
s1725696 | 3:f87769ba4a9b | 57 | |
s1725696 | 0:83a0d1ae0768 | 58 | // Potmeter 2 |
s1725696 | 3:f87769ba4a9b | 59 | float out_3 = b; |
s1725696 | 3:f87769ba4a9b | 60 | b = (out_3*2.0f) - 1.0f; |
s1725696 | 3:f87769ba4a9b | 61 | pc.printf("out_4 = %f ", b); // + or - y-direction |
s1725696 | 3:f87769ba4a9b | 62 | } |
s1725696 | 3:f87769ba4a9b | 63 | |
s1725696 | 3:f87769ba4a9b | 64 | void encoder(volatile double& a, volatile double&b) |
s1725696 | 3:f87769ba4a9b | 65 | { |
s1725696 | 3:f87769ba4a9b | 66 | a = Encoder1.getPulses(); |
s1725696 | 3:f87769ba4a9b | 67 | b = Encoder2.getPulses(); |
s1725696 | 3:f87769ba4a9b | 68 | pc.printf("counts1=%i counts2=%i ", counts1,counts2); |
s1725696 | 3:f87769ba4a9b | 69 | } |
s1725696 | 0:83a0d1ae0768 | 70 | |
s1725696 | 3:f87769ba4a9b | 71 | void direction_prep(volatile double& a, volatile double& b, volatile double& c, volatile double& d) // a=positie_x, b=positie_y, c=theta, d=A |
s1725696 | 3:f87769ba4a9b | 72 | { |
s1725696 | 0:83a0d1ae0768 | 73 | // verwerking potmeter output tot richtingen motor |
s1725696 | 3:f87769ba4a9b | 74 | a = pot_1*20.0*delta_t; // de max is 20 mm/s |
s1725696 | 3:f87769ba4a9b | 75 | b = pot_2*20.0*delta_t; |
s1725696 | 3:f87769ba4a9b | 76 | c = gamma*counts2; |
s1725696 | 3:f87769ba4a9b | 77 | Lpc_x = (r-(alpha*counts1+l_beta))*sin(c); |
s1725696 | 3:f87769ba4a9b | 78 | Lpc_y = (r-(alpha*counts1+l_beta))*cos(c); |
s1725696 | 3:f87769ba4a9b | 79 | d = sqrt(pow(Lpc_x,2)+pow(Lpc_y,2)); |
s1725696 | 3:f87769ba4a9b | 80 | |
s1725696 | 3:f87769ba4a9b | 81 | pc.printf("positie_x=%f positie_y=%f ",positie_x,positie_y); |
s1725696 | 3:f87769ba4a9b | 82 | } |
s1725696 | 3:f87769ba4a9b | 83 | |
s1725696 | 3:f87769ba4a9b | 84 | void x_direction(volatile double& a, volatile double& b) // a=dc2_x, b=dc1_x |
s1725696 | 3:f87769ba4a9b | 85 | { |
s1725696 | 3:f87769ba4a9b | 86 | a = (asin(positie_x/A)/gamma)-counts2; |
s1725696 | 3:f87769ba4a9b | 87 | delta_y = A*cos(gamma*dc2_x+theta); |
s1725696 | 3:f87769ba4a9b | 88 | b = (r-(delta_y/cos(theta))-l_beta)/alpha; |
s1725696 | 0:83a0d1ae0768 | 89 | |
s1725696 | 3:f87769ba4a9b | 90 | pc.printf("dc2_x=%f dc1_x=%f ",dc2_x,dc1_x); |
s1725696 | 3:f87769ba4a9b | 91 | } |
s1725696 | 3:f87769ba4a9b | 92 | |
s1725696 | 3:f87769ba4a9b | 93 | void y_direction(volatile double& a, volatile double& b) // a=dc2_y, b=dc1_y |
s1725696 | 3:f87769ba4a9b | 94 | { |
s1725696 | 3:f87769ba4a9b | 95 | a = (acos(positie_y/A)/gamma)-counts2; |
s1725696 | 3:f87769ba4a9b | 96 | delta_x = A*sin(gamma*dc2_y+theta); |
s1725696 | 3:f87769ba4a9b | 97 | b = (r-(delta_x/cos(theta))-l_beta)/alpha; |
s1725696 | 0:83a0d1ae0768 | 98 | |
s1725696 | 3:f87769ba4a9b | 99 | pc.printf("dc2_y=%f dc1_y=%f ",dc2_y,dc1_y); |
s1725696 | 3:f87769ba4a9b | 100 | } |
s1725696 | 3:f87769ba4a9b | 101 | |
s1725696 | 3:f87769ba4a9b | 102 | void counts_to_pwm() |
s1725696 | 3:f87769ba4a9b | 103 | { |
s1725696 | 3:f87769ba4a9b | 104 | // going from counts to pwm |
s1725696 | 3:f87769ba4a9b | 105 | speed_desired_motor1 = ((dc1_x+dc1_y)*alpha)/delta_t; // for max 2cm/s translation -> range pwm: -1/pi - 1/pi |
s1725696 | 3:f87769ba4a9b | 106 | speed_desired_motor2 = ((dc2_x+dc2_y)*gamma)/delta_t; // for max rotation -> range pwm: -2/4,65 - 2/4,65 |
s1725696 | 3:f87769ba4a9b | 107 | max_speed_motor2 = 46.5; // CALCULATIONS NEEDED!!! |
s1725696 | 0:83a0d1ae0768 | 108 | |
s1725696 | 1:b9595e136a00 | 109 | pc.printf("speed1=%f speed2=%f ", speed_desired_motor1,speed_desired_motor2); |
s1725696 | 0:83a0d1ae0768 | 110 | |
s1725696 | 1:b9595e136a00 | 111 | double pwm1 = speed_desired_motor1/(pi); // 20.0 mm/s |
s1725696 | 1:b9595e136a00 | 112 | double pwm2 = (speed_desired_motor2*2)/(max_speed_motor2); |
s1725696 | 0:83a0d1ae0768 | 113 | |
s1725696 | 2:3f849fd62ebb | 114 | pc.printf("pwm1=%f pwm2=%f\r\n",pwm1,pwm2); |
s1725696 | 0:83a0d1ae0768 | 115 | |
s1725696 | 3:f87769ba4a9b | 116 | // giving directions to motors |
s1725696 | 0:83a0d1ae0768 | 117 | dirpin.write(pwm2 < 0); |
s1725696 | 0:83a0d1ae0768 | 118 | pwmpin = fabs (pwm2); |
s1725696 | 0:83a0d1ae0768 | 119 | dirpin_2.write(pwm1 < 0); |
s1725696 | 3:f87769ba4a9b | 120 | pwmpin_2 = fabs (pwm1); |
s1725696 | 3:f87769ba4a9b | 121 | } |
s1725696 | 3:f87769ba4a9b | 122 | |
s1725696 | 3:f87769ba4a9b | 123 | void rki_big_function() |
s1725696 | 3:f87769ba4a9b | 124 | { |
s1725696 | 3:f87769ba4a9b | 125 | float a = pot_1; |
s1725696 | 3:f87769ba4a9b | 126 | float b = pot_2; |
s1725696 | 3:f87769ba4a9b | 127 | potmeter(a,b); // zet potmeter waarden om naar goede range (-1 - 1) |
s1725696 | 3:f87769ba4a9b | 128 | encoder(counts1,counts2); // getting counts from encoder |
s1725696 | 3:f87769ba4a9b | 129 | direction_prep(positie_x, positie_y, theta, A); // a=positie_x, b=positie_y, c=theta, d=A |
s1725696 | 3:f87769ba4a9b | 130 | x_direction(dc2_x,dc1_x); // counts for change in x-direction |
s1725696 | 3:f87769ba4a9b | 131 | y_direction(dc2_y,dc1_y); // counts for change in y-direction |
s1725696 | 3:f87769ba4a9b | 132 | counts_to_pwm(); // changing counts to pwm |
s1725696 | 3:f87769ba4a9b | 133 | } |
s1725696 | 3:f87769ba4a9b | 134 | |
s1725696 | 3:f87769ba4a9b | 135 | |
s1725696 | 3:f87769ba4a9b | 136 | // main |
s1725696 | 3:f87769ba4a9b | 137 | // ----------------------------------------------------------------------------- |
s1725696 | 3:f87769ba4a9b | 138 | int main() |
s1725696 | 3:f87769ba4a9b | 139 | { |
s1725696 | 3:f87769ba4a9b | 140 | pc.baud(115200); // setting baudrate |
s1725696 | 3:f87769ba4a9b | 141 | pc.printf("start\r\n"); // to see if the program starts |
s1725696 | 3:f87769ba4a9b | 142 | pwmpin.period_us(60); // setting the pwm period |
s1725696 | 3:f87769ba4a9b | 143 | |
s1725696 | 3:f87769ba4a9b | 144 | while(true) |
s1725696 | 3:f87769ba4a9b | 145 | { |
s1725696 | 3:f87769ba4a9b | 146 | rki_big_function(); |
s1725696 | 0:83a0d1ae0768 | 147 | |
s1725696 | 3:f87769ba4a9b | 148 | wait(delta_t); // aantal keer dat een signaal wordt gecheckt en doorgestuurd |
s1725696 | 3:f87769ba4a9b | 149 | } |
s1725696 | 0:83a0d1ae0768 | 150 | } |
s1725696 | 0:83a0d1ae0768 | 151 |