Sp2019_EW202_3321_1 / Mbed 2 deprecated gradient

gradient

Dependencies:   mbed TCS3472_I2C Tach ContinuousServo

main.cpp@0:46d75bfc2492, 2019-04-27 (annotated)

Committer:
m215910
Date:
Sat Apr 27 00:47:12 2019 +0000
Revision:
0:46d75bfc2492
gradient

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m215910 0:46d75bfc2492 1 //Satre Gagnon, Ewing, Batten
m215910 0:46d75bfc2492 2 //Drive Straight
m215910 0:46d75bfc2492 3
m215910 0:46d75bfc2492 4 #include "mbed.h"
m215910 0:46d75bfc2492 5 #include "ContinuousServo.h"
m215910 0:46d75bfc2492 6 #include "Tach.h"
m215910 0:46d75bfc2492 7 #include "TCS3472_I2C.h"
m215910 0:46d75bfc2492 8
m215910 0:46d75bfc2492 9 // color sensor
m215910 0:46d75bfc2492 10 DigitalOut hallpwr(p22); //PWM out signal to power LED on the sensor
m215910 0:46d75bfc2492 11 DigitalIn hall(p21);
m215910 0:46d75bfc2492 12 TCS3472_I2C rgb_sensor(p9, p10); // Establish RGB sensor object
m215910 0:46d75bfc2492 13 Serial pc(USBTX,USBRX); //Establish serial connection
m215910 0:46d75bfc2492 14 int rgb_data[4]; //store sensor readings
m215910 0:46d75bfc2492 15
m215910 0:46d75bfc2492 16 //lights for colors
m215910 0:46d75bfc2492 17 DigitalOut red(LED1);
m215910 0:46d75bfc2492 18 DigitalOut green(LED2);
m215910 0:46d75bfc2492 19 DigitalOut blue(LED3);
m215910 0:46d75bfc2492 20 DigitalOut lightmag(LED4);
m215910 0:46d75bfc2492 21
m215910 0:46d75bfc2492 22 //speed
m215910 0:46d75bfc2492 23 float wL, wR;
m215910 0:46d75bfc2492 24 float x = 0.3, y = -0.3, dist, realdist;
m215910 0:46d75bfc2492 25
m215910 0:46d75bfc2492 26 ContinuousServo left(p23);
m215910 0:46d75bfc2492 27 ContinuousServo right(p26);
m215910 0:46d75bfc2492 28
m215910 0:46d75bfc2492 29 //encoders
m215910 0:46d75bfc2492 30 Tach tLeft(p17,64);
m215910 0:46d75bfc2492 31 Tach tRight(p13,64);
m215910 0:46d75bfc2492 32
m215910 0:46d75bfc2492 33 int main()
m215910 0:46d75bfc2492 34 {
m215910 0:46d75bfc2492 35 rgb_sensor.enablePowerAndRGBC(); //Enable RGB sensor
m215910 0:46d75bfc2492 36 rgb_sensor.setIntegrationTime(100); //Set integration time of sensor
m215910 0:46d75bfc2492 37
m215910 0:46d75bfc2492 38 wL = tLeft.getSpeed(); //speed of left wheel (rev/sec)
m215910 0:46d75bfc2492 39 wR = tRight.getSpeed(); //speed of right wheel (rev/sec)
m215910 0:46d75bfc2492 40
m215910 0:46d75bfc2492 41 pc.baud(9600); //set baud rate
m215910 0:46d75bfc2492 42 float PWMbrightness = 1.0; //float specifying brightness of LED (between 0.0 and 1.0)
m215910 0:46d75bfc2492 43
m215910 0:46d75bfc2492 44 while(1) {
m215910 0:46d75bfc2492 45
m215910 0:46d75bfc2492 46 left.speed(x);
m215910 0:46d75bfc2492 47 right.speed(y); //right negative to go forward
m215910 0:46d75bfc2492 48
m215910 0:46d75bfc2492 49 hallpwr = PWMbrightness; //set brightness of sensor LED
m215910 0:46d75bfc2492 50 rgb_sensor.getAllColors(rgb_data);
m215910 0:46d75bfc2492 51
m215910 0:46d75bfc2492 52 if ((rgb_data[0] > 10000))
m215910 0:46d75bfc2492 53 {
m215910 0:46d75bfc2492 54 blue = 0;
m215910 0:46d75bfc2492 55 green = 0;
m215910 0:46d75bfc2492 56 red = 0;
m215910 0:46d75bfc2492 57 }
m215910 0:46d75bfc2492 58
m215910 0:46d75bfc2492 59 else if ((rgb_data[1] > rgb_data[2]) && (rgb_data[1] > rgb_data[3]) )
m215910 0:46d75bfc2492 60 {
m215910 0:46d75bfc2492 61 red = 1;
m215910 0:46d75bfc2492 62 green = 0;
m215910 0:46d75bfc2492 63 blue = 0;
m215910 0:46d75bfc2492 64 }
m215910 0:46d75bfc2492 65 else if ((rgb_data[2] > rgb_data[1]) && (rgb_data[2] > rgb_data[3]) )
m215910 0:46d75bfc2492 66 {
m215910 0:46d75bfc2492 67 if(rgb_data[2] > 1100)
m215910 0:46d75bfc2492 68 {
m215910 0:46d75bfc2492 69 green = 1;
m215910 0:46d75bfc2492 70 red = 0;
m215910 0:46d75bfc2492 71 blue = 0;
m215910 0:46d75bfc2492 72 }
m215910 0:46d75bfc2492 73 else
m215910 0:46d75bfc2492 74 {
m215910 0:46d75bfc2492 75 green = 0;
m215910 0:46d75bfc2492 76 red = 0;
m215910 0:46d75bfc2492 77 blue = 0;
m215910 0:46d75bfc2492 78 }
m215910 0:46d75bfc2492 79 }
m215910 0:46d75bfc2492 80
m215910 0:46d75bfc2492 81 else if ((rgb_data[3] > rgb_data[2]) && (rgb_data[3] > rgb_data[1]) )
m215910 0:46d75bfc2492 82 {
m215910 0:46d75bfc2492 83 blue = 1;
m215910 0:46d75bfc2492 84 green = 0;
m215910 0:46d75bfc2492 85 red = 0;
m215910 0:46d75bfc2492 86 }
m215910 0:46d75bfc2492 87 if (hall == 1){
m215910 0:46d75bfc2492 88 lightmag = 1;
m215910 0:46d75bfc2492 89 printf("BOMB DETECTED");
m215910 0:46d75bfc2492 90 }
m215910 0:46d75bfc2492 91 else if (hall == 0){
m215910 0:46d75bfc2492 92 lightmag = 0;
m215910 0:46d75bfc2492 93 }
m215910 0:46d75bfc2492 94 if (wL<wR) {
m215910 0:46d75bfc2492 95 x = x+0.01;
m215910 0:46d75bfc2492 96 y = y; }
m215910 0:46d75bfc2492 97 else if (wR<wL) {
m215910 0:46d75bfc2492 98 x = x-0.01;
m215910 0:46d75bfc2492 99 y = y;}
m215910 0:46d75bfc2492 100 else if (wR == wL) {
m215910 0:46d75bfc2492 101 x = x;
m215910 0:46d75bfc2492 102 y = y;
m215910 0:46d75bfc2492 103 }
m215910 0:46d75bfc2492 104 if (realdist <= 8) {
m215910 0:46d75bfc2492 105 left.stop();
m215910 0:46d75bfc2492 106 right.stop();
m215910 0:46d75bfc2492 107 }
m215910 0:46d75bfc2492 108 if ((rgb_data[0] > 9000))
m215910 0:46d75bfc2492 109 {
m215910 0:46d75bfc2492 110 x = x;
m215910 0:46d75bfc2492 111 y = 0;
m215910 0:46d75bfc2492 112 }
m215910 0:46d75bfc2492 113 if ((rgb_data[0] < 800))
m215910 0:46d75bfc2492 114 {
m215910 0:46d75bfc2492 115 x = x;
m215910 0:46d75bfc2492 116 y = 0;
m215910 0:46d75bfc2492 117 }
m215910 0:46d75bfc2492 118
m215910 0:46d75bfc2492 119
m215910 0:46d75bfc2492 120 printf("unfiltered: %d, red: %d, green: %d, blue: %d \n", rgb_data[0], rgb_data[1], rgb_data[2], rgb_data[3]);
m215910 0:46d75bfc2492 121
m215910 0:46d75bfc2492 122 } //while
m215910 0:46d75bfc2492 123 } //main