2nd draft
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed Servo
Fork of robot_mockup by
Diff: main.cpp
- Revision:
- 54:4cda9af56bed
- Parent:
- 53:bf0d97487e84
- Child:
- 55:726fdab812a9
diff -r bf0d97487e84 -r 4cda9af56bed main.cpp --- a/main.cpp Tue Oct 27 10:04:46 2015 +0000 +++ b/main.cpp Tue Oct 27 10:11:29 2015 +0000 @@ -2,6 +2,7 @@ #include "HIDScope.h" #include "MODSERIAL.h" #include "biquadFilter.h" +#include "Servo.h" #include "QEI.h" #include "math.h" #include <string> @@ -177,7 +178,8 @@ //Forward Kinematics const double l1 = 0.25; const double l2 = 0.25; //Arm lengths double current_x; double current_y; //Current position -double theta1; double theta2; //Current angles +double theta1; double theta2; double theta3; //Current angles +double servopos; //servo position in usec double rpc; //Encoder resolution: radians per count //Reference position @@ -1052,7 +1054,8 @@ //Servo alignment theta3 = 180 - theta1 - theta2; - servoPwm.SetPosition = (2100/180)*theta3 + 600; + servopos = (2100/180)*theta3 + 600; + servoPwm.SetPosition(servopos); /*if(u1 > 0) {