code
Dependencies: ContinuousServo Final Project ES202 KANG TRUONG ADAMS TCS3472_I2C Tach
PROJECT.cpp
- Committer:
- kingkang2
- Date:
- 2018-04-19
- Revision:
- 3:8d12cc699e4c
- Parent:
- 2:9ab67bed1a34
File content as of revision 3:8d12cc699e4c:
#include "mbed.h" #include "ContinuousServo.h" #include "Tach.h" #include "TCS3472_I2C.h" Serial pc(USBTX,USBRX);// color sensor TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object 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 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; }