![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 33:88fbf14d8aaf
- Parent:
- 32:60a71dcfdb7a
- Child:
- 34:0af87f9cfb7b
- Child:
- 35:4cb2ed6dd2d2
diff -r 60a71dcfdb7a -r 88fbf14d8aaf Motor_tryout.cpp --- a/Motor_tryout.cpp Thu Oct 31 14:30:59 2019 +0000 +++ b/Motor_tryout.cpp Thu Oct 31 15:02:17 2019 +0000 @@ -3,14 +3,15 @@ #include "QEI.h" #include "Math.h" #include "ttmath.h" +#include "FastPWM.h" MODSERIAL pc(USBTX, USBRX); //Serial term (USBTX, USBRX); -PwmOut motor1_pwm(PTC2); +FastPWM motor1_pwm(PTC2); DigitalOut motor1_dir(PTC3); -PwmOut motor2_pwm(PTA2); +FastPWM motor2_pwm(PTA2); DigitalOut motor2_dir(PTB23); -PwmOut motor3_pwm(PTC4); +FastPWM motor3_pwm(PTC4); DigitalOut motor3_dir(PTC12); AnalogIn potmeter1(A1); @@ -190,7 +191,12 @@ counts2 = Encoder2.getPulses(); counts3 = Encoder3.getPulses(); - z0 = -172.0 + 10* sin(time_sin); +// z0 = -172.0 + 10* sin(time_sin); + +float r = 20*(1-1/(1+(time_sin))); + x0 = r*sin(6.28 * time_sin); + y0 = r*cos(6.28 * time_sin); + delta_calctheta1 (); delta_calctheta2 (); @@ -204,10 +210,9 @@ error2 = theta2 - angle2; error3 = theta3 - angle3; - Kp = 10; - Kd = potmeter1 * 1000; + Kp = potmeter1 * 25; - u_k1 = Kp * error1; + u_k1 = Kp * error1; u_k2 = Kp * error2; u_k3 = Kp * error3; @@ -261,8 +266,13 @@ char cc = pc.getc(); int frequency_pwm = 10000; //10 kHz PWM + motor1_pwm.period_us(6); + motor2_pwm.period_us(6); + motor3_pwm.period_us(6); while(true){ + + } //END MAIN } \ No newline at end of file