Final Project ES202 KANG TRUONG ADAMS / DREAMTEAM

code

Dependencies:   ContinuousServo Final Project ES202 KANG TRUONG ADAMS TCS3472_I2C Tach

PROJECT.cpp@3:8d12cc699e4c, 2018-04-19 (annotated)

Committer:
kingkang2
Date:
Thu Apr 19 15:29:36 2018 +0000
Revision:
3:8d12cc699e4c
Parent:
2:9ab67bed1a34 
straight line;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kingkang2 0:b33656ee16a7 1 #include "mbed.h"
kingkang2 0:b33656ee16a7 2 #include "ContinuousServo.h"
kingkang2 0:b33656ee16a7 3 #include "Tach.h"
kingkang2 0:b33656ee16a7 4 #include "TCS3472_I2C.h"
kingkang2 0:b33656ee16a7 5
kingkang2 3:8d12cc699e4c 6 Serial pc(USBTX,USBRX);// color sensor
kingkang2 0:b33656ee16a7 7 TCS3472_I2C rgb_sensor( p9, p10 ); // Establish RGB sensor object
kingkang2 3:8d12cc699e4c 8 DigitalIn hall(p21);// Hall effect sensor
kingkang2 3:8d12cc699e4c 9 DigitalOut hallpwr(p22);
kingkang2 3:8d12cc699e4c 10 AnalogIn sonar(p19); // sonar sensor, range sensor 9.8 mV/inch
kingkang2 3:8d12cc699e4c 11
kingkang2 0:b33656ee16a7 12 //servos
kingkang2 0:b33656ee16a7 13 ContinuousServo left(p23);
kingkang2 0:b33656ee16a7 14 ContinuousServo right(p26);
kingkang2 3:8d12cc699e4c 15
kingkang2 0:b33656ee16a7 16 //encoders
kingkang2 0:b33656ee16a7 17 Tach tLeft(p17,64);
kingkang2 1:8a2bdbd69fba 18 Tach tRight(p13,64);
kingkang2 1:8a2bdbd69fba 19
kingkang2 3:8d12cc699e4c 20 // functions
kingkang2 3:8d12cc699e4c 21 float calcPWM(float desired_velocity, float velocity);
kingkang2 3:8d12cc699e4c 22
kingkang2 3:8d12cc699e4c 23 // initialize variables
kingkang2 3:8d12cc699e4c 24 w = 0;
kingkang2 3:8d12cc699e4c 25 v = 0.5;
kingkang2 3:8d12cc699e4c 26 r = 3.3;
kingkang2 3:8d12cc699e4c 27 phi = 90;
kingkang2 3:8d12cc699e4c 28 l = 4.1;
kingkang2 3:8d12cc699e4c 29 float left_rps;
kingkang2 3:8d12cc699e4c 30 float right_rps;
kingkang2 3:8d12cc699e4c 31 Vright = (2v+ wl)/2r; // rad/s
kingkang2 3:8d12cc699e4c 32 Vleft = (2v - wl)/2r; // rad/s
kingkang2 3:8d12cc699e4c 33
kingkang2 3:8d12cc699e4c 34 void forward(){
kingkang2 3:8d12cc699e4c 35
kingkang2 3:8d12cc699e4c 36 }
kingkang2 3:8d12cc699e4c 37
kingkang2 3:8d12cc699e4c 38 void stop(){
kingkang2 3:8d12cc699e4c 39
kingkang2 3:8d12cc699e4c 40 }
kingkang2 3:8d12cc699e4c 41
kingkang2 3:8d12cc699e4c 42 int main()
kingkang2 3:8d12cc699e4c 43 {
kingkang2 3:8d12cc699e4c 44
kingkang2 3:8d12cc699e4c 45
kingkang2 1:8a2bdbd69fba 46 //drive straight
kingkang2 1:8a2bdbd69fba 47
kingkang2 3:8d12cc699e4c 48 while(1) {
kingkang2 3:8d12cc699e4c 49
kingkang2 3:8d12cc699e4c 50 right = Vright;
kingkang2 3:8d12cc699e4c 51 left = Vleft;
kingkang2 3:8d12cc699e4c 52 right_rps = tRight.read() * 2* pi; //rad/s
kingkang2 3:8d12cc699e4c 53 left_rps = tLeft.read() * 2 * pi; //rad/s
kingkang2 3:8d12cc699e4c 54 sDC_right = calcPWM(Vright,right_rps); // SET DUTY CYCLE FOR RIGHT SERVO
kingkang2 3:8d12cc699e4c 55 sDC_left = calcPWM(Vleft,left_rps); // SET DUTY CYCLE FOR LEFT SERVO
kingkang2 3:8d12cc699e4c 56 right.speed(sDC_right);
kingkang2 3:8d12cc699e4c 57 left.speed(sDC_left);
kingkang2 3:8d12cc699e4c 58 wait(0.05);
kingkang2 3:8d12cc699e4c 59
kingkang2 3:8d12cc699e4c 60
kingkang2 3:8d12cc699e4c 61
kingkang2 3:8d12cc699e4c 62
kingkang2 3:8d12cc699e4c 63 }
kingkang2 3:8d12cc699e4c 64 }
kingkang2 3:8d12cc699e4c 65
kingkang2 3:8d12cc699e4c 66
kingkang2 3:8d12cc699e4c 67 // calculates the PWM duty cycle given the desired and measured velocity
kingkang2 3:8d12cc699e4c 68 float calcPWM(float desired_velocity, float velocity)
kingkang2 3:8d12cc699e4c 69 {
kingkang2 3:8d12cc699e4c 70
kingkang2 3:8d12cc699e4c 71 float v_err;
kingkang2 3:8d12cc699e4c 72 // velocity error
kingkang2 3:8d12cc699e4c 73 v_err = (desired_velocity - velocity);
kingkang2 3:8d12cc699e4c 74
kingkang2 3:8d12cc699e4c 75 sDC = .039*v_err; // enter duty cycle calculation here!!
kingkang2 3:8d12cc699e4c 76 return sDC;
kingkang2 3:8d12cc699e4c 77 }