codevoor esther
Dependencies: Encoder MODSERIAL mbed
main.cpp@1:0db79ea80741, 2013-11-04 (annotated)
- Committer:
- gerjan
- Date:
- Mon Nov 04 14:21:07 2013 +0000
- Revision:
- 1:0db79ea80741
- Parent:
- 0:155294201f36
- Child:
- 2:3987ed9570c8
vrijdag werkend;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gerjan | 0:155294201f36 | 1 | #include "mbed.h" |
gerjan | 0:155294201f36 | 2 | #include "encoder.h" |
gerjan | 0:155294201f36 | 3 | #include "MODSERIAL.h" |
gerjan | 0:155294201f36 | 4 | |
gerjan | 0:155294201f36 | 5 | // Maken van een looptimer. |
gerjan | 0:155294201f36 | 6 | volatile bool looptimerflag; |
gerjan | 0:155294201f36 | 7 | void setlooptimerflag(void) |
gerjan | 0:155294201f36 | 8 | { |
gerjan | 0:155294201f36 | 9 | looptimerflag = true; |
gerjan | 0:155294201f36 | 10 | } |
gerjan | 0:155294201f36 | 11 | |
gerjan | 0:155294201f36 | 12 | int main() |
gerjan | 0:155294201f36 | 13 | { |
gerjan | 0:155294201f36 | 14 | |
gerjan | 0:155294201f36 | 15 | // Communicatie voor pc |
gerjan | 0:155294201f36 | 16 | MODSERIAL pc(USBTX,USBRX); |
gerjan | 0:155294201f36 | 17 | pc.baud(115200); |
gerjan | 0:155294201f36 | 18 | |
gerjan | 0:155294201f36 | 19 | // Voor aansturen motoren |
gerjan | 0:155294201f36 | 20 | double pi; |
gerjan | 0:155294201f36 | 21 | pi=3.14159265359; |
gerjan | 0:155294201f36 | 22 | |
gerjan | 0:155294201f36 | 23 | double x,y; |
gerjan | 0:155294201f36 | 24 | |
gerjan | 0:155294201f36 | 25 | |
gerjan | 0:155294201f36 | 26 | |
gerjan | 0:155294201f36 | 27 | // Variabelen benoemen voor regelaar motor. |
gerjan | 0:155294201f36 | 28 | double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm; |
gerjan | 0:155294201f36 | 29 | double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm; |
gerjan | 0:155294201f36 | 30 | double motor1_maxu, motor2_maxu; |
gerjan | 0:155294201f36 | 31 | double Ts; |
gerjan | 0:155294201f36 | 32 | |
gerjan | 0:155294201f36 | 33 | // Sample tijd |
gerjan | 0:155294201f36 | 34 | Ts = 0.001; |
gerjan | 0:155294201f36 | 35 | |
gerjan | 0:155294201f36 | 36 | // Pinnen voor potmeter |
gerjan | 0:155294201f36 | 37 | AnalogIn potmeter1(PTB2); |
gerjan | 0:155294201f36 | 38 | AnalogIn potmeter2(PTB3); |
gerjan | 0:155294201f36 | 39 | |
gerjan | 0:155294201f36 | 40 | // Pinnen voor encoder |
gerjan | 0:155294201f36 | 41 | /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */ |
gerjan | 0:155294201f36 | 42 | Encoder motor1(PTD0,PTC9); |
gerjan | 0:155294201f36 | 43 | Encoder motor2(PTD2,PTC8); |
gerjan | 0:155294201f36 | 44 | |
gerjan | 0:155294201f36 | 45 | /* PWM naar motor */ |
gerjan | 0:155294201f36 | 46 | PwmOut pwm_motor1(PTA12); |
gerjan | 0:155294201f36 | 47 | PwmOut pwm_motor2(PTA5); |
gerjan | 0:155294201f36 | 48 | |
gerjan | 0:155294201f36 | 49 | /* Pin voor richting */ |
gerjan | 0:155294201f36 | 50 | DigitalOut motor1dir(PTD3); |
gerjan | 0:155294201f36 | 51 | DigitalOut motor2dir(PTD1); |
gerjan | 0:155294201f36 | 52 | |
gerjan | 0:155294201f36 | 53 | //Tijd looptimer instellen. |
gerjan | 0:155294201f36 | 54 | Ticker looptimer; |
gerjan | 0:155294201f36 | 55 | looptimer.attach(setlooptimerflag,Ts); |
gerjan | 0:155294201f36 | 56 | |
gerjan | 0:155294201f36 | 57 | |
gerjan | 0:155294201f36 | 58 | // Oneidige loop... |
gerjan | 0:155294201f36 | 59 | while(true) { |
gerjan | 0:155294201f36 | 60 | while(looptimerflag != true); |
gerjan | 0:155294201f36 | 61 | looptimerflag = false; |
gerjan | 0:155294201f36 | 62 | |
gerjan | 1:0db79ea80741 | 63 | x = (potmeter1.read()*297.0+69.8)*-1.0; |
gerjan | 0:155294201f36 | 64 | y = (potmeter2.read()*210.0+69.8); |
gerjan | 0:155294201f36 | 65 | |
gerjan | 1:0db79ea80741 | 66 | //x = x*10.0 + 69.8; |
gerjan | 1:0db79ea80741 | 67 | //y = y*10.0 + 69.8; |
gerjan | 0:155294201f36 | 68 | |
gerjan | 0:155294201f36 | 69 | theta = atan(y/x) ;// * (400.0/(.5*pi)); |
gerjan | 0:155294201f36 | 70 | r = (sqrt(x*x+y*y)) ;// * (2577/461.335); |
gerjan | 0:155294201f36 | 71 | |
gerjan | 0:155294201f36 | 72 | |
gerjan | 0:155294201f36 | 73 | theta_pen = motor1.getPosition() * ((.5*pi)/400); |
gerjan | 1:0db79ea80741 | 74 | r_pen = motor2.getPosition() * (363.0/2577.0); |
gerjan | 0:155294201f36 | 75 | |
gerjan | 0:155294201f36 | 76 | |
gerjan | 0:155294201f36 | 77 | dtheta = (theta - theta_pen); |
gerjan | 0:155294201f36 | 78 | dr = (r - r_pen); |
gerjan | 0:155294201f36 | 79 | |
gerjan | 1:0db79ea80741 | 80 | theta_pwm = (dtheta)*0.5; |
gerjan | 0:155294201f36 | 81 | r_pwm = (dr)*0.001; |
gerjan | 0:155294201f36 | 82 | |
gerjan | 0:155294201f36 | 83 | //NAAR MOTOR |
gerjan | 0:155294201f36 | 84 | |
gerjan | 0:155294201f36 | 85 | //Zorgen dat pwm tussen -1 en 1 blijft. |
gerjan | 0:155294201f36 | 86 | if(theta_pwm > 1) { |
gerjan | 0:155294201f36 | 87 | theta_pwm=1; |
gerjan | 0:155294201f36 | 88 | } |
gerjan | 0:155294201f36 | 89 | if(theta_pwm < -1) { |
gerjan | 0:155294201f36 | 90 | theta_pwm=-1; |
gerjan | 0:155294201f36 | 91 | } |
gerjan | 0:155294201f36 | 92 | if(r_pwm > 1) { |
gerjan | 0:155294201f36 | 93 | r_pwm=1; |
gerjan | 0:155294201f36 | 94 | } |
gerjan | 0:155294201f36 | 95 | if(r_pwm < -1) { |
gerjan | 0:155294201f36 | 96 | r_pwm=-1; |
gerjan | 0:155294201f36 | 97 | } |
gerjan | 0:155294201f36 | 98 | |
gerjan | 0:155294201f36 | 99 | // Bepaal richting waarin motoren moeten draaien |
gerjan | 0:155294201f36 | 100 | if(theta_pwm > 0) |
gerjan | 0:155294201f36 | 101 | motor1dir.write(1); |
gerjan | 0:155294201f36 | 102 | else |
gerjan | 0:155294201f36 | 103 | motor1dir.write(0); |
gerjan | 0:155294201f36 | 104 | if(r_pwm > 0) |
gerjan | 0:155294201f36 | 105 | motor2dir.write(1); |
gerjan | 0:155294201f36 | 106 | else |
gerjan | 0:155294201f36 | 107 | motor2dir.write(0); |
gerjan | 0:155294201f36 | 108 | |
gerjan | 0:155294201f36 | 109 | // print naar pc |
gerjan | 1:0db79ea80741 | 110 | pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm); |
gerjan | 0:155294201f36 | 111 | |
gerjan | 0:155294201f36 | 112 | //schrijf PWM naar motor |
gerjan | 0:155294201f36 | 113 | pwm_motor1.write(abs(theta_pwm)); |
gerjan | 0:155294201f36 | 114 | pwm_motor2.write(abs(r_pwm)); |
gerjan | 0:155294201f36 | 115 | |
gerjan | 0:155294201f36 | 116 | |
gerjan | 0:155294201f36 | 117 | |
gerjan | 0:155294201f36 | 118 | } |
gerjan | 0:155294201f36 | 119 | } |