2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 49:6515c045cfd6
- Parent:
- 48:a1f97eb8c020
- Child:
- 50:54f71544964c
--- a/main.cpp Fri Oct 23 20:40:31 2015 +0000 +++ b/main.cpp Fri Oct 23 21:09:32 2015 +0000 @@ -14,8 +14,8 @@ #define PI 3.14159265 #define SAMPLE_RATE 0.002 //500 Hz EMG sample rate #define CONTROL_RATE 0.01 //100 Hz Control rate -#define ENCODER1_CPR 4200 //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft -#define ENCODER2_CPR 4200 //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2), +#define ENCODER_CPR 4200 //both motor encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft + //gearbox 1:131.25 -> 4200 counts per revolution of the output shaft (X2), #define PWM_PERIOD 0.0001 //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly /*-------------------------------------------------------------------------------------------------------------------- ---- OBJECTS --------------------------------------------------------------------------------------------------------- @@ -928,7 +928,7 @@ //Current position - Encoder counts -> current angle -> Forward Kinematics - rpc=(2*PI)/ENCODER1_CPR; //radians per count (resolution) - 2pi divided by 4200 + rpc=(2*PI)/ENCODER_CPR; //radians per count (resolution) - 2pi divided by 4200 theta1 = Encoder1.getPulses() * rpc; //multiply resolution with number of counts to get current angles theta2 = Encoder2.getPulses() * rpc; current_x = l1 * cos(theta1) + l2 * cos(theta1 + theta2); //Forward kinematics for current position