Group 9 BioRobotics / Mbed 2 deprecated rki_try_out

alleen x-richting, speed rete hoog

Dependencies:   QEI mbed

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?

UserRevisionLine numberNew 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