code
Dependencies: ContinuousServo Final Project ES202 KANG TRUONG ADAMS TCS3472_I2C Tach
Diff: PROJECT.cpp
- Revision:
- 3:8d12cc699e4c
- Parent:
- 2:9ab67bed1a34
--- 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; +}