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