codevoor esther
Dependencies: Encoder MODSERIAL mbed
main.cpp
- Committer:
- gerjan
- Date:
- 2013-11-04
- Revision:
- 1:0db79ea80741
- Parent:
- 0:155294201f36
- Child:
- 2:3987ed9570c8
File content as of revision 1:0db79ea80741:
#include "mbed.h" #include "encoder.h" #include "MODSERIAL.h" // Maken van een looptimer. volatile bool looptimerflag; void setlooptimerflag(void) { looptimerflag = true; } int main() { // Communicatie voor pc MODSERIAL pc(USBTX,USBRX); pc.baud(115200); // Voor aansturen motoren double pi; pi=3.14159265359; double x,y; // Variabelen benoemen voor regelaar motor. 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; 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; double motor1_maxu, motor2_maxu; double Ts; // Sample tijd Ts = 0.001; // Pinnen voor potmeter AnalogIn potmeter1(PTB2); AnalogIn potmeter2(PTB3); // Pinnen voor encoder /* First pin should be PTDx or PTAx because those pins can be used as interruptIn */ Encoder motor1(PTD0,PTC9); Encoder motor2(PTD2,PTC8); /* PWM naar motor */ PwmOut pwm_motor1(PTA12); PwmOut pwm_motor2(PTA5); /* Pin voor richting */ DigitalOut motor1dir(PTD3); DigitalOut motor2dir(PTD1); //Tijd looptimer instellen. Ticker looptimer; looptimer.attach(setlooptimerflag,Ts); // Oneidige loop... while(true) { while(looptimerflag != true); looptimerflag = false; x = (potmeter1.read()*297.0+69.8)*-1.0; y = (potmeter2.read()*210.0+69.8); //x = x*10.0 + 69.8; //y = y*10.0 + 69.8; theta = atan(y/x) ;// * (400.0/(.5*pi)); r = (sqrt(x*x+y*y)) ;// * (2577/461.335); theta_pen = motor1.getPosition() * ((.5*pi)/400); r_pen = motor2.getPosition() * (363.0/2577.0); dtheta = (theta - theta_pen); dr = (r - r_pen); theta_pwm = (dtheta)*0.5; r_pwm = (dr)*0.001; //NAAR MOTOR //Zorgen dat pwm tussen -1 en 1 blijft. if(theta_pwm > 1) { theta_pwm=1; } if(theta_pwm < -1) { theta_pwm=-1; } if(r_pwm > 1) { r_pwm=1; } if(r_pwm < -1) { r_pwm=-1; } // Bepaal richting waarin motoren moeten draaien if(theta_pwm > 0) motor1dir.write(1); else motor1dir.write(0); if(r_pwm > 0) motor2dir.write(1); else motor2dir.write(0); // print naar pc pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm); //schrijf PWM naar motor pwm_motor1.write(abs(theta_pwm)); pwm_motor2.write(abs(r_pwm)); } }