![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Hall 0900
Dependencies: mbed TCS3472_I2C Tach ContinuousServo
main.cpp@0:3c7038dd80b3, 2019-04-26 (annotated)
- Committer:
- m211482
- Date:
- Fri Apr 26 21:03:49 2019 +0000
- Revision:
- 0:3c7038dd80b3
ONLY WORKS ON HALL0900;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m211482 | 0:3c7038dd80b3 | 1 | #include "mbed.h" |
m211482 | 0:3c7038dd80b3 | 2 | #include "TCS3472_I2C.h" |
m211482 | 0:3c7038dd80b3 | 3 | #include "ContinuousServo.h" |
m211482 | 0:3c7038dd80b3 | 4 | #include "Tach.h" |
m211482 | 0:3c7038dd80b3 | 5 | |
m211482 | 0:3c7038dd80b3 | 6 | |
m211482 | 0:3c7038dd80b3 | 7 | Serial pc(USBTX,USBRX); |
m211482 | 0:3c7038dd80b3 | 8 | PwmOut LB(p22); |
m211482 | 0:3c7038dd80b3 | 9 | TCS3472_I2C rgb_sensor(p9, p10); |
m211482 | 0:3c7038dd80b3 | 10 | ContinuousServo left(p23); |
m211482 | 0:3c7038dd80b3 | 11 | ContinuousServo right(p26); |
m211482 | 0:3c7038dd80b3 | 12 | float turn, pt; |
m211482 | 0:3c7038dd80b3 | 13 | float r,l; |
m211482 | 0:3c7038dd80b3 | 14 | //ecnoders |
m211482 | 0:3c7038dd80b3 | 15 | Tach tLeft(p17,64); |
m211482 | 0:3c7038dd80b3 | 16 | Tach tRight(p13,64); |
m211482 | 0:3c7038dd80b3 | 17 | int main() |
m211482 | 0:3c7038dd80b3 | 18 | { |
m211482 | 0:3c7038dd80b3 | 19 | rgb_sensor.enablePowerAndRGBC(); |
m211482 | 0:3c7038dd80b3 | 20 | rgb_sensor.setIntegrationTime(100); |
m211482 | 0:3c7038dd80b3 | 21 | |
m211482 | 0:3c7038dd80b3 | 22 | pc.baud(9600); |
m211482 | 0:3c7038dd80b3 | 23 | int rgb_data[4]; |
m211482 | 0:3c7038dd80b3 | 24 | float PWMbrightness=1.0; |
m211482 | 0:3c7038dd80b3 | 25 | right.speed(-0.1); |
m211482 | 0:3c7038dd80b3 | 26 | left.speed(0.1); |
m211482 | 0:3c7038dd80b3 | 27 | while (1) { |
m211482 | 0:3c7038dd80b3 | 28 | LB=PWMbrightness; |
m211482 | 0:3c7038dd80b3 | 29 | rgb_sensor.getAllColors(rgb_data); |
m211482 | 0:3c7038dd80b3 | 30 | pc.printf("unfiltered: %d, red: %d, green: %d, blue: %d \n\r", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]); |
m211482 | 0:3c7038dd80b3 | 31 | turn=(750-rgb_data[0]); |
m211482 | 0:3c7038dd80b3 | 32 | pt=0.00038*(turn); |
m211482 | 0:3c7038dd80b3 | 33 | if (pt>0.25) { |
m211482 | 0:3c7038dd80b3 | 34 | pt=0.25; |
m211482 | 0:3c7038dd80b3 | 35 | } |
m211482 | 0:3c7038dd80b3 | 36 | if (pt<-0.25) { |
m211482 | 0:3c7038dd80b3 | 37 | pt=-0.25; |
m211482 | 0:3c7038dd80b3 | 38 | } |
m211482 | 0:3c7038dd80b3 | 39 | r=-0.1-(pt/2); |
m211482 | 0:3c7038dd80b3 | 40 | l=0.1-(pt/2); |
m211482 | 0:3c7038dd80b3 | 41 | right.speed(r); |
m211482 | 0:3c7038dd80b3 | 42 | left.speed(l); |
m211482 | 0:3c7038dd80b3 | 43 | pc.printf("right %f, left %f\n\r",r,l); |
m211482 | 0:3c7038dd80b3 | 44 | } |
m211482 | 0:3c7038dd80b3 | 45 | } |
m211482 | 0:3c7038dd80b3 | 46 | /* if ((rgb_data[0]<=800)&&(rgb_data[0]>=740)) { |
m211482 | 0:3c7038dd80b3 | 47 | pc.printf("Go straight, %d \n\r",rgb_data[0]); |
m211482 | 0:3c7038dd80b3 | 48 | } else if (rgb_data[0]<740) { |
m211482 | 0:3c7038dd80b3 | 49 | pc.printf("Turn left, %d\n\r",rgb_data[0]); |
m211482 | 0:3c7038dd80b3 | 50 | } else if (rgb_data[0]>800) { |
m211482 | 0:3c7038dd80b3 | 51 | pc.printf("Turn right, %d \n\r",rgb_data[0]); |
m211482 | 0:3c7038dd80b3 | 52 | } |
m211482 | 0:3c7038dd80b3 | 53 | wait (0.5); |
m211482 | 0:3c7038dd80b3 | 54 | } |
m211482 | 0:3c7038dd80b3 | 55 | }*/ |