Gerjan Wolterink / Mbed 2 deprecated Pregelaar

codevoor esther

Dependencies:   Encoder MODSERIAL mbed

main.cpp@10:36281503362a, 2013-11-04 (annotated)

Committer:
gerjan
Date:
Mon Nov 04 21:25:46 2013 +0000
Revision:
10:36281503362a
Parent:
9:ca6c6295b5f1 
voor naar de emg;

Who changed what in which revision?

UserRevisionLine numberNew 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 10:36281503362a 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, dri, dri_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 8:e3e972c4e249 58 motor2.setPosition(597.15);
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 10:36281503362a 64 ki_r = 0.005;
gerjan 0:155294201f36 65
gerjan 6:bea0424b407c 66 inputsinus=0;
gerjan 10:36281503362a 67 dri=0;
gerjan 0:155294201f36 68
gerjan 0:155294201f36 69 // Oneidige loop...
gerjan 0:155294201f36 70 while(true) {
gerjan 0:155294201f36 71 while(looptimerflag != true);
gerjan 0:155294201f36 72 looptimerflag = false;
gerjan 0:155294201f36 73
gerjan 4:863a52425322 74 //x = (potmeter1.read()*297.0);
gerjan 4:863a52425322 75 //y = (potmeter2.read()*210.0);
gerjan 0:155294201f36 76
gerjan 9:ca6c6295b5f1 77 x = 0;//sin(inputsinus)*297.0;
gerjan 9:ca6c6295b5f1 78 y = sin(inputsinus)*210.0;
gerjan 6:bea0424b407c 79
gerjan 10:36281503362a 80 inputsinus = inputsinus + (Ts*0.5)*pi;
gerjan 6:bea0424b407c 81
gerjan 8:e3e972c4e249 82 //Binnen bereik blijven
gerjan 6:bea0424b407c 83
gerjan 7:422b8ec97278 84 if (y>210.0) {
gerjan 7:422b8ec97278 85 y=210.0;
gerjan 6:bea0424b407c 86 }
gerjan 7:422b8ec97278 87 if(y<=0.0) {
gerjan 7:422b8ec97278 88 y=0.0;
gerjan 6:bea0424b407c 89 }
gerjan 7:422b8ec97278 90 if(x>297.0) {
gerjan 7:422b8ec97278 91 x=297.0;
gerjan 6:bea0424b407c 92 }
gerjan 7:422b8ec97278 93 if(x<=0.0) {
gerjan 7:422b8ec97278 94 x=0.0;
gerjan 6:bea0424b407c 95 }
gerjan 6:bea0424b407c 96
gerjan 7:422b8ec97278 97 // Omzetten hoek en straal
gerjan 7:422b8ec97278 98 //theta = atan(y/x)+0.25*pi;
gerjan 7:422b8ec97278 99 //r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
gerjan 7:422b8ec97278 100
gerjan 9:ca6c6295b5f1 101 theta = atan((x+69.80)/(y+69.80));
gerjan 8:e3e972c4e249 102 r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
gerjan 6:bea0424b407c 103
gerjan 0:155294201f36 104
gerjan 3:0edffb90e739 105 theta_pen = motor1.getPosition() * ((.5*pi)/400.0);
gerjan 8:e3e972c4e249 106 r_pen = motor2.getPosition() * (461.34/2790.13);
gerjan 6:bea0424b407c 107
gerjan 6:bea0424b407c 108
gerjan 0:155294201f36 109 dtheta = (theta - theta_pen);
gerjan 0:155294201f36 110 dr = (r - r_pen);
gerjan 6:bea0424b407c 111
gerjan 5:8f3530006712 112 //REGELAAR
gerjan 6:bea0424b407c 113 up_r = kp_r * dr; //P-actie
gerjan 10:36281503362a 114 dri = dri_1 + dr*Ts;
gerjan 10:36281503362a 115 ui_r = dri_1 * ki_r; //I-actie
gerjan 6:bea0424b407c 116 utot_r = up_r + ui_r; //
gerjan 0:155294201f36 117
gerjan 10:36281503362a 118 dri_1 = dri; //nieuwe waardes oud maken.
gerjan 6:bea0424b407c 119
gerjan 6:bea0424b407c 120 theta_pwm = (dtheta)*3.0;
gerjan 5:8f3530006712 121 r_pwm = (utot_r/1.0);
gerjan 0:155294201f36 122
gerjan 0:155294201f36 123 //NAAR MOTOR
gerjan 6:bea0424b407c 124
gerjan 0:155294201f36 125 //Zorgen dat pwm tussen -1 en 1 blijft.
gerjan 0:155294201f36 126 if(theta_pwm > 1) {
gerjan 0:155294201f36 127 theta_pwm=1;
gerjan 0:155294201f36 128 }
gerjan 0:155294201f36 129 if(theta_pwm < -1) {
gerjan 0:155294201f36 130 theta_pwm=-1;
gerjan 0:155294201f36 131 }
gerjan 0:155294201f36 132 if(r_pwm > 1) {
gerjan 0:155294201f36 133 r_pwm=1;
gerjan 0:155294201f36 134 }
gerjan 0:155294201f36 135 if(r_pwm < -1) {
gerjan 0:155294201f36 136 r_pwm=-1;
gerjan 0:155294201f36 137 }
gerjan 0:155294201f36 138
gerjan 0:155294201f36 139 // Bepaal richting waarin motoren moeten draaien
gerjan 0:155294201f36 140 if(theta_pwm > 0)
gerjan 0:155294201f36 141 motor1dir.write(1);
gerjan 0:155294201f36 142 else
gerjan 0:155294201f36 143 motor1dir.write(0);
gerjan 0:155294201f36 144 if(r_pwm > 0)
gerjan 0:155294201f36 145 motor2dir.write(1);
gerjan 0:155294201f36 146 else
gerjan 0:155294201f36 147 motor2dir.write(0);
gerjan 6:bea0424b407c 148
gerjan 0:155294201f36 149 // print naar pc
gerjan 4:863a52425322 150 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 151
gerjan 6:bea0424b407c 152 //schrijf PWM naar motor
gerjan 0:155294201f36 153 pwm_motor1.write(abs(theta_pwm));
gerjan 0:155294201f36 154 pwm_motor2.write(abs(r_pwm));
gerjan 0:155294201f36 155
gerjan 0:155294201f36 156
gerjan 0:155294201f36 157
gerjan 0:155294201f36 158 }
gerjan 0:155294201f36 159 }