code
Dependencies: ContinuousServo Final Project ES202 KANG TRUONG ADAMS TCS3472_I2C Tach
Revision 3:8d12cc699e4c, committed 2018-04-19
- Comitter:
- kingkang2
- Date:
- Thu Apr 19 15:29:36 2018 +0000
- Parent:
- 2:9ab67bed1a34
- Commit message:
- straight line;
Changed in this revision
PROJECT.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 9ab67bed1a34 -r 8d12cc699e4c PROJECT.cpp --- a/PROJECT.cpp Thu Apr 19 14:31:10 2018 +0000 +++ b/PROJECT.cpp Thu Apr 19 15:29:36 2018 +0000 @@ -3,21 +3,75 @@ #include "Tach.h" #include "TCS3472_I2C.h" -Serial pc(USBTX,USBRX); -// color sensor +Serial pc(USBTX,USBRX);// color sensor TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object -// Hall effect sensor -DigitalIn hall(p22); -// sonar sensor -AnalogIn sonar(p20); // range sensor 9.8 mV/inch +DigitalIn hall(p21);// Hall effect sensor +DigitalOut hallpwr(p22); +AnalogIn sonar(p19); // sonar sensor, range sensor 9.8 mV/inch + //servos ContinuousServo left(p23); ContinuousServo right(p26); + //encoders Tach tLeft(p17,64); Tach tRight(p13,64); +// functions +float calcPWM(float desired_velocity, float velocity); + +// initialize variables + w = 0; + v = 0.5; + r = 3.3; + phi = 90; + l = 4.1; + float left_rps; + float right_rps; + Vright = (2v+ wl)/2r; // rad/s + Vleft = (2v - wl)/2r; // rad/s + +void forward(){ + +} + +void stop(){ + +} + +int main() +{ + + //drive straight -w = 0; -v = 0.5; + while(1) { + + right = Vright; + left = Vleft; + right_rps = tRight.read() * 2* pi; //rad/s + left_rps = tLeft.read() * 2 * pi; //rad/s + sDC_right = calcPWM(Vright,right_rps); // SET DUTY CYCLE FOR RIGHT SERVO + sDC_left = calcPWM(Vleft,left_rps); // SET DUTY CYCLE FOR LEFT SERVO + right.speed(sDC_right); + left.speed(sDC_left); + wait(0.05); + + + + + } +} + + +// calculates the PWM duty cycle given the desired and measured velocity +float calcPWM(float desired_velocity, float velocity) +{ + + float v_err; + // velocity error + v_err = (desired_velocity - velocity); + + sDC = .039*v_err; // enter duty cycle calculation here!! + return sDC; +}