codevoor esther
Dependencies: Encoder MODSERIAL mbed
main.cpp@6:bea0424b407c, 2013-11-04 (annotated)
- Committer:
- gerjan
- Date:
- Mon Nov 04 19:58:59 2013 +0000
- Revision:
- 6:bea0424b407c
- Parent:
- 5:8f3530006712
- Child:
- 7:422b8ec97278
Voor Esthers idee;
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 | 6:bea0424b407c | 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, dr_1, utot_r, inputsinus; |
gerjan | 0:155294201f36 | 30 | double motor1_maxu, motor2_maxu; |
gerjan | 0:155294201f36 | 31 | double Ts; |
gerjan | 6:bea0424b407c | 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 | 6:bea0424b407c | 56 | |
gerjan | 3:0edffb90e739 | 57 | motor1.setPosition(200.0); |
gerjan | 2:3987ed9570c8 | 58 | motor2.setPosition(0); |
gerjan | 2:3987ed9570c8 | 59 | x=0; |
gerjan | 2:3987ed9570c8 | 60 | y=0; |
gerjan | 2:3987ed9570c8 | 61 | |
gerjan | 6:bea0424b407c | 62 | kp_r = 0.006; |
gerjan | 6:bea0424b407c | 63 | ki_r = 0.015; |
gerjan | 0:155294201f36 | 64 | |
gerjan | 6:bea0424b407c | 65 | inputsinus=0; |
gerjan | 0:155294201f36 | 66 | |
gerjan | 0:155294201f36 | 67 | // Oneidige loop... |
gerjan | 0:155294201f36 | 68 | while(true) { |
gerjan | 0:155294201f36 | 69 | while(looptimerflag != true); |
gerjan | 0:155294201f36 | 70 | looptimerflag = false; |
gerjan | 0:155294201f36 | 71 | |
gerjan | 4:863a52425322 | 72 | //x = (potmeter1.read()*297.0); |
gerjan | 4:863a52425322 | 73 | //y = (potmeter2.read()*210.0); |
gerjan | 0:155294201f36 | 74 | |
gerjan | 4:863a52425322 | 75 | x = sin(inputsinus)*297; |
gerjan | 4:863a52425322 | 76 | y = 0; |
gerjan | 6:bea0424b407c | 77 | |
gerjan | 6:bea0424b407c | 78 | inputsinus = inputsinus + (Ts*1)*pi; |
gerjan | 6:bea0424b407c | 79 | |
gerjan | 6:bea0424b407c | 80 | //Binnen berijk blijven |
gerjan | 6:bea0424b407c | 81 | |
gerjan | 6:bea0424b407c | 82 | if (y>210) { |
gerjan | 6:bea0424b407c | 83 | y=210 |
gerjan | 6:bea0424b407c | 84 | } |
gerjan | 6:bea0424b407c | 85 | if(y<=0) { |
gerjan | 6:bea0424b407c | 86 | y=0 |
gerjan | 6:bea0424b407c | 87 | } |
gerjan | 6:bea0424b407c | 88 | if(x>297) { |
gerjan | 6:bea0424b407c | 89 | x=297 |
gerjan | 6:bea0424b407c | 90 | } |
gerjan | 6:bea0424b407c | 91 | if(x<=0) { |
gerjan | 6:bea0424b407c | 92 | x=0 |
gerjan | 6:bea0424b407c | 93 | } |
gerjan | 6:bea0424b407c | 94 | |
gerjan | 6:bea0424b407c | 95 | // Omzetten hoek en straal |
gerjan | 6:bea0424b407c | 96 | theta = atan(y/x)+0.25*pi; |
gerjan | 3:0edffb90e739 | 97 | r = (sqrt(x*x+y*y)) ;// * (2577/461.335); |
gerjan | 6:bea0424b407c | 98 | |
gerjan | 0:155294201f36 | 99 | |
gerjan | 3:0edffb90e739 | 100 | theta_pen = motor1.getPosition() * ((.5*pi)/400.0); |
gerjan | 3:0edffb90e739 | 101 | r_pen = motor2.getPosition() * (363.0/2196.0); |
gerjan | 6:bea0424b407c | 102 | |
gerjan | 6:bea0424b407c | 103 | |
gerjan | 0:155294201f36 | 104 | dtheta = (theta - theta_pen); |
gerjan | 0:155294201f36 | 105 | dr = (r - r_pen); |
gerjan | 6:bea0424b407c | 106 | |
gerjan | 5:8f3530006712 | 107 | //REGELAAR |
gerjan | 6:bea0424b407c | 108 | up_r = kp_r * dr; //P-actie |
gerjan | 6:bea0424b407c | 109 | ui_r = (dr_1 + (dr * Ts)) * ki_r; //I-actie |
gerjan | 6:bea0424b407c | 110 | utot_r = up_r + ui_r; // |
gerjan | 0:155294201f36 | 111 | |
gerjan | 6:bea0424b407c | 112 | dr_1 = dr; //nieuwe waardes oud maken. |
gerjan | 6:bea0424b407c | 113 | |
gerjan | 6:bea0424b407c | 114 | theta_pwm = (dtheta)*3.0; |
gerjan | 5:8f3530006712 | 115 | r_pwm = (utot_r/1.0); |
gerjan | 0:155294201f36 | 116 | |
gerjan | 0:155294201f36 | 117 | //NAAR MOTOR |
gerjan | 6:bea0424b407c | 118 | |
gerjan | 0:155294201f36 | 119 | //Zorgen dat pwm tussen -1 en 1 blijft. |
gerjan | 0:155294201f36 | 120 | if(theta_pwm > 1) { |
gerjan | 0:155294201f36 | 121 | theta_pwm=1; |
gerjan | 0:155294201f36 | 122 | } |
gerjan | 0:155294201f36 | 123 | if(theta_pwm < -1) { |
gerjan | 0:155294201f36 | 124 | theta_pwm=-1; |
gerjan | 0:155294201f36 | 125 | } |
gerjan | 0:155294201f36 | 126 | if(r_pwm > 1) { |
gerjan | 0:155294201f36 | 127 | r_pwm=1; |
gerjan | 0:155294201f36 | 128 | } |
gerjan | 0:155294201f36 | 129 | if(r_pwm < -1) { |
gerjan | 0:155294201f36 | 130 | r_pwm=-1; |
gerjan | 0:155294201f36 | 131 | } |
gerjan | 0:155294201f36 | 132 | |
gerjan | 0:155294201f36 | 133 | // Bepaal richting waarin motoren moeten draaien |
gerjan | 0:155294201f36 | 134 | if(theta_pwm > 0) |
gerjan | 0:155294201f36 | 135 | motor1dir.write(1); |
gerjan | 0:155294201f36 | 136 | else |
gerjan | 0:155294201f36 | 137 | motor1dir.write(0); |
gerjan | 0:155294201f36 | 138 | if(r_pwm > 0) |
gerjan | 0:155294201f36 | 139 | motor2dir.write(1); |
gerjan | 0:155294201f36 | 140 | else |
gerjan | 0:155294201f36 | 141 | motor2dir.write(0); |
gerjan | 6:bea0424b407c | 142 | |
gerjan | 0:155294201f36 | 143 | // print naar pc |
gerjan | 4:863a52425322 | 144 | pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus); |
gerjan | 0:155294201f36 | 145 | |
gerjan | 6:bea0424b407c | 146 | //schrijf PWM naar motor |
gerjan | 0:155294201f36 | 147 | pwm_motor1.write(abs(theta_pwm)); |
gerjan | 0:155294201f36 | 148 | pwm_motor2.write(abs(r_pwm)); |
gerjan | 0:155294201f36 | 149 | |
gerjan | 0:155294201f36 | 150 | |
gerjan | 0:155294201f36 | 151 | |
gerjan | 0:155294201f36 | 152 | } |
gerjan | 0:155294201f36 | 153 | } |