Gerjan Wolterink / Mbed 2 deprecated Pregelaar

codevoor esther

Dependencies:   Encoder MODSERIAL mbed

main.cpp@0:155294201f36, 2013-11-01 (annotated)

Committer:
gerjan
Date:
Fri Nov 01 15:34:03 2013 +0000
Revision:
0:155294201f36
Child:
1:0db79ea80741
draait heel hard;

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 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
gerjan 0:155294201f36 32 double Ts;
gerjan 0:155294201f36 33
gerjan 0:155294201f36 34 // Sample tijd
gerjan 0:155294201f36 35 Ts = 0.001;
gerjan 0:155294201f36 36
gerjan 0:155294201f36 37 // Pinnen voor potmeter
gerjan 0:155294201f36 38 AnalogIn potmeter1(PTB2);
gerjan 0:155294201f36 39 AnalogIn potmeter2(PTB3);
gerjan 0:155294201f36 40
gerjan 0:155294201f36 41 // Pinnen voor encoder
gerjan 0:155294201f36 42 /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */
gerjan 0:155294201f36 43 Encoder motor1(PTD0,PTC9);
gerjan 0:155294201f36 44 Encoder motor2(PTD2,PTC8);
gerjan 0:155294201f36 45
gerjan 0:155294201f36 46 /* PWM naar motor */
gerjan 0:155294201f36 47 PwmOut pwm_motor1(PTA12);
gerjan 0:155294201f36 48 PwmOut pwm_motor2(PTA5);
gerjan 0:155294201f36 49
gerjan 0:155294201f36 50 /* Pin voor richting */
gerjan 0:155294201f36 51 DigitalOut motor1dir(PTD3);
gerjan 0:155294201f36 52 DigitalOut motor2dir(PTD1);
gerjan 0:155294201f36 53
gerjan 0:155294201f36 54 //Tijd looptimer instellen.
gerjan 0:155294201f36 55 Ticker looptimer;
gerjan 0:155294201f36 56 looptimer.attach(setlooptimerflag,Ts);
gerjan 0:155294201f36 57
gerjan 0:155294201f36 58
gerjan 0:155294201f36 59 // Oneidige loop...
gerjan 0:155294201f36 60 while(true) {
gerjan 0:155294201f36 61 while(looptimerflag != true);
gerjan 0:155294201f36 62 looptimerflag = false;
gerjan 0:155294201f36 63
gerjan 0:155294201f36 64 x = (potmeter1.read()*297.0+69.8);
gerjan 0:155294201f36 65 y = (potmeter2.read()*210.0+69.8);
gerjan 0:155294201f36 66
gerjan 0:155294201f36 67
gerjan 0:155294201f36 68 theta = atan(y/x) ;// * (400.0/(.5*pi));
gerjan 0:155294201f36 69 r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
gerjan 0:155294201f36 70
gerjan 0:155294201f36 71
gerjan 0:155294201f36 72 theta_pen = motor1.getPosition() * ((.5*pi)/400);
gerjan 0:155294201f36 73 r_pen = motor2.getPosition() * (363/2577);
gerjan 0:155294201f36 74
gerjan 0:155294201f36 75
gerjan 0:155294201f36 76 dtheta = (theta - theta_pen);
gerjan 0:155294201f36 77 dr = (r - r_pen);
gerjan 0:155294201f36 78
gerjan 0:155294201f36 79 dtheta =
gerjan 0:155294201f36 80
gerjan 0:155294201f36 81 theta_pwm = (dtheta)*0.001;
gerjan 0:155294201f36 82 r_pwm = (dr)*0.001;
gerjan 0:155294201f36 83
gerjan 0:155294201f36 84 //NAAR MOTOR
gerjan 0:155294201f36 85
gerjan 0:155294201f36 86 //Zorgen dat pwm tussen -1 en 1 blijft.
gerjan 0:155294201f36 87 if(theta_pwm > 1) {
gerjan 0:155294201f36 88 theta_pwm=1;
gerjan 0:155294201f36 89 }
gerjan 0:155294201f36 90 if(theta_pwm < -1) {
gerjan 0:155294201f36 91 theta_pwm=-1;
gerjan 0:155294201f36 92 }
gerjan 0:155294201f36 93 if(r_pwm > 1) {
gerjan 0:155294201f36 94 r_pwm=1;
gerjan 0:155294201f36 95 }
gerjan 0:155294201f36 96 if(r_pwm < -1) {
gerjan 0:155294201f36 97 r_pwm=-1;
gerjan 0:155294201f36 98 }
gerjan 0:155294201f36 99
gerjan 0:155294201f36 100 // Bepaal richting waarin motoren moeten draaien
gerjan 0:155294201f36 101 if(theta_pwm > 0)
gerjan 0:155294201f36 102 motor1dir.write(1);
gerjan 0:155294201f36 103 else
gerjan 0:155294201f36 104 motor1dir.write(0);
gerjan 0:155294201f36 105 if(r_pwm > 0)
gerjan 0:155294201f36 106 motor2dir.write(1);
gerjan 0:155294201f36 107 else
gerjan 0:155294201f36 108 motor2dir.write(0);
gerjan 0:155294201f36 109
gerjan 0:155294201f36 110 // print naar pc
gerjan 0:155294201f36 111 pc.printf("t=%.3f dt=%.3f tpwm=%.3f\n", theta, dtheta, theta_pwm);
gerjan 0:155294201f36 112
gerjan 0:155294201f36 113 //schrijf PWM naar motor
gerjan 0:155294201f36 114 pwm_motor1.write(abs(theta_pwm));
gerjan 0:155294201f36 115 pwm_motor2.write(abs(r_pwm));
gerjan 0:155294201f36 116
gerjan 0:155294201f36 117
gerjan 0:155294201f36 118
gerjan 0:155294201f36 119 }
gerjan 0:155294201f36 120 }