codevoor esther
Dependencies: Encoder MODSERIAL mbed
main.cpp
- Committer:
- gerjan
- Date:
- 2013-11-04
- Revision:
- 6:bea0424b407c
- Parent:
- 5:8f3530006712
- Child:
- 7:422b8ec97278
File content as of revision 6:bea0424b407c:
#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, dr_1, utot_r, inputsinus; 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); motor1.setPosition(200.0); motor2.setPosition(0); x=0; y=0; kp_r = 0.006; ki_r = 0.015; inputsinus=0; // Oneidige loop... while(true) { while(looptimerflag != true); looptimerflag = false; //x = (potmeter1.read()*297.0); //y = (potmeter2.read()*210.0); x = sin(inputsinus)*297; y = 0; inputsinus = inputsinus + (Ts*1)*pi; //Binnen berijk blijven if (y>210) { y=210 } if(y<=0) { y=0 } if(x>297) { x=297 } if(x<=0) { x=0 } // Omzetten hoek en straal theta = atan(y/x)+0.25*pi; r = (sqrt(x*x+y*y)) ;// * (2577/461.335); theta_pen = motor1.getPosition() * ((.5*pi)/400.0); r_pen = motor2.getPosition() * (363.0/2196.0); dtheta = (theta - theta_pen); dr = (r - r_pen); //REGELAAR up_r = kp_r * dr; //P-actie ui_r = (dr_1 + (dr * Ts)) * ki_r; //I-actie utot_r = up_r + ui_r; // dr_1 = dr; //nieuwe waardes oud maken. theta_pwm = (dtheta)*3.0; r_pwm = (utot_r/1.0); //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 inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus); //schrijf PWM naar motor pwm_motor1.write(abs(theta_pwm)); pwm_motor2.write(abs(r_pwm)); } }