codevoor esther
Dependencies: Encoder MODSERIAL mbed
Diff: main.cpp
- Revision:
- 4:863a52425322
- Parent:
- 3:0edffb90e739
- Child:
- 5:8f3530006712
diff -r 0edffb90e739 -r 863a52425322 main.cpp --- a/main.cpp Mon Nov 04 16:08:46 2013 +0000 +++ b/main.cpp Mon Nov 04 16:29:51 2013 +0000 @@ -26,7 +26,7 @@ // 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 r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, inputsinus; double motor1_maxu, motor2_maxu; double Ts; @@ -66,24 +66,14 @@ while(looptimerflag != true); looptimerflag = false; - - x = (potmeter1.read()*297.0); - y = (potmeter2.read()*210.0); - - //x = x*10.0 + 69.8; - //y = y*10.0 + 69.8; + //x = (potmeter1.read()*297.0); + //y = (potmeter2.read()*210.0); - if(x < 30){ - theta = 0.25*pi; - } - if(y < 21){ - theta = 0.25*pi; - } + x = sin(inputsinus)*297; + y = 0; - if(x >= 30 && y >= 21){ - theta = atan(y/x)+(.25*pi);// * (400.0/(.5*pi)); - } - + inputsinus = inputsinus + Ts*pi; + theta = 0.25*pi; r = (sqrt(x*x+y*y)) ;// * (2577/461.335); @@ -94,8 +84,8 @@ dtheta = (theta - theta_pen); dr = (r - r_pen); - theta_pwm = (dtheta)*0.5; - r_pwm = (dr)*0.001; + theta_pwm = (dtheta)*3; + r_pwm = (dr)*0.0015; //NAAR MOTOR @@ -124,9 +114,9 @@ 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); + 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 + //schrijf PWM naar motor pwm_motor1.write(abs(theta_pwm)); pwm_motor2.write(abs(r_pwm));