pierre masala
/
Servo
nmbbghgj
Fork of Servo by
main.cpp@4:707923a648dc, 2018-01-05 (annotated)
- Committer:
- pierre11
- Date:
- Fri Jan 05 16:22:50 2018 +0000
- Revision:
- 4:707923a648dc
- Parent:
- 3:9640475cdfbe
fin ;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pierre11 | 2:49ad86fca628 | 1 | #include "mbed.h" |
pierre11 | 2:49ad86fca628 | 2 | #include "Servo.h" |
pierre11 | 4:707923a648dc | 3 | #include "TCS3200.h" |
pierre11 | 4:707923a648dc | 4 | |
pierre11 | 4:707923a648dc | 5 | TCS3200 color(PF_6, PF_7, PF_8, PF_9, PA_0); //Create a TCS3200 object |
pierre11 | 4:707923a648dc | 6 | // S0 S1 S2 S3 OUT |
pierre11 | 4:707923a648dc | 7 | |
pierre11 | 4:707923a648dc | 8 | Servo Servo2(PD_12); |
pierre11 | 4:707923a648dc | 9 | Servo Servo3(PD_13); |
pierre11 | 4:707923a648dc | 10 | Servo Servo4(PD_14); |
pierre11 | 4:707923a648dc | 11 | Servo Servo5(PD_15); |
pierre11 | 4:707923a648dc | 12 | |
pierre11 | 4:707923a648dc | 13 | void move_1(); |
pierre11 | 4:707923a648dc | 14 | void move_2(); |
pierre11 | 4:707923a648dc | 15 | void move_3(); |
pierre11 | 4:707923a648dc | 16 | void move_4(); |
pierre11 | 4:707923a648dc | 17 | |
pierre11 | 4:707923a648dc | 18 | DigitalOut myled1(LED1); |
pierre11 | 4:707923a648dc | 19 | DigitalOut myled2(LED2); |
pierre11 | 4:707923a648dc | 20 | DigitalOut myled3(LED3); |
pierre11 | 4:707923a648dc | 21 | |
pierre11 | 4:707923a648dc | 22 | int colour(); |
pierre11 | 4:707923a648dc | 23 | |
pierre11 | 2:49ad86fca628 | 24 | int main(int argc, char* argv[]) |
pierre11 | 2:49ad86fca628 | 25 | { |
pierre11 | 4:707923a648dc | 26 | |
pierre11 | 4:707923a648dc | 27 | //Set the scaling factor to 100% |
pierre11 | 4:707923a648dc | 28 | //color.SetMode(TCS3200::SCALE_100); |
pierre11 | 4:707923a648dc | 29 | myled1 = 0; |
pierre11 | 4:707923a648dc | 30 | myled2 = 0; |
pierre11 | 4:707923a648dc | 31 | myled3 = 0; |
pierre11 | 4:707923a648dc | 32 | |
pierre11 | 4:707923a648dc | 33 | |
pierre11 | 4:707923a648dc | 34 | Servo5.Enable(1500,20000); //turn |
pierre11 | 4:707923a648dc | 35 | Servo2.Enable(1500,20000); // forward |
pierre11 | 4:707923a648dc | 36 | Servo3.Enable(1500,20000); |
pierre11 | 4:707923a648dc | 37 | Servo4.Enable(1500,20000); // backward |
pierre11 | 4:707923a648dc | 38 | |
pierre11 | 4:707923a648dc | 39 | // |
pierre11 | 2:49ad86fca628 | 40 | |
pierre11 | 4:707923a648dc | 41 | while(1) |
pierre11 | 4:707923a648dc | 42 | { |
pierre11 | 4:707923a648dc | 43 | //1 |
pierre11 | 4:707923a648dc | 44 | move_1(); |
pierre11 | 4:707923a648dc | 45 | move_2(); |
pierre11 | 4:707923a648dc | 46 | move_3(); |
pierre11 | 4:707923a648dc | 47 | move_4(); |
pierre11 | 4:707923a648dc | 48 | if(colour()== 1) |
pierre11 | 4:707923a648dc | 49 | { |
pierre11 | 4:707923a648dc | 50 | printf("RED\n"); |
pierre11 | 4:707923a648dc | 51 | myled1 = 0; |
pierre11 | 4:707923a648dc | 52 | myled2 = 0; |
pierre11 | 4:707923a648dc | 53 | myled3 = 1; |
pierre11 | 4:707923a648dc | 54 | |
pierre11 | 4:707923a648dc | 55 | |
pierre11 | 4:707923a648dc | 56 | } |
pierre11 | 4:707923a648dc | 57 | else |
pierre11 | 4:707923a648dc | 58 | { |
pierre11 | 4:707923a648dc | 59 | if(colour() == 2) |
pierre11 | 4:707923a648dc | 60 | { |
pierre11 | 4:707923a648dc | 61 | printf("GREEN\n"); |
pierre11 | 4:707923a648dc | 62 | myled1 = 1; |
pierre11 | 4:707923a648dc | 63 | myled2 = 0; |
pierre11 | 4:707923a648dc | 64 | myled3 = 0; |
pierre11 | 4:707923a648dc | 65 | } |
pierre11 | 4:707923a648dc | 66 | else |
pierre11 | 4:707923a648dc | 67 | { |
pierre11 | 4:707923a648dc | 68 | printf("BLUE\n"); |
pierre11 | 4:707923a648dc | 69 | myled1 = 0; |
pierre11 | 4:707923a648dc | 70 | myled2 = 1; |
pierre11 | 4:707923a648dc | 71 | myled3 = 0; |
pierre11 | 4:707923a648dc | 72 | } |
pierre11 | 4:707923a648dc | 73 | } |
pierre11 | 4:707923a648dc | 74 | |
pierre11 | 4:707923a648dc | 75 | |
pierre11 | 4:707923a648dc | 76 | } |
pierre11 | 4:707923a648dc | 77 | |
pierre11 | 4:707923a648dc | 78 | return 0; |
pierre11 | 4:707923a648dc | 79 | } |
pierre11 | 4:707923a648dc | 80 | |
pierre11 | 4:707923a648dc | 81 | void move_1() |
pierre11 | 4:707923a648dc | 82 | { |
pierre11 | 4:707923a648dc | 83 | for (int pos = 500; pos < 2000; pos += 25) |
pierre11 | 2:49ad86fca628 | 84 | { |
pierre11 | 4:707923a648dc | 85 | Servo4.SetPosition(pos); |
pierre11 | 2:49ad86fca628 | 86 | wait_ms(20); |
pierre11 | 2:49ad86fca628 | 87 | } |
pierre11 | 4:707923a648dc | 88 | |
pierre11 | 4:707923a648dc | 89 | for (int pos = 2000; pos > 500; pos -= 25) |
pierre11 | 4:707923a648dc | 90 | { |
pierre11 | 4:707923a648dc | 91 | Servo4.SetPosition(pos); |
pierre11 | 2:49ad86fca628 | 92 | wait_ms(20); |
pierre11 | 4:707923a648dc | 93 | } |
pierre11 | 4:707923a648dc | 94 | } |
pierre11 | 4:707923a648dc | 95 | void move_2() |
pierre11 | 4:707923a648dc | 96 | { |
pierre11 | 4:707923a648dc | 97 | for (int pos = 500; pos < 2000; pos += 25) |
pierre11 | 3:9640475cdfbe | 98 | { |
pierre11 | 3:9640475cdfbe | 99 | Servo2.SetPosition(pos); |
pierre11 | 3:9640475cdfbe | 100 | wait_ms(20); |
pierre11 | 3:9640475cdfbe | 101 | } |
pierre11 | 4:707923a648dc | 102 | |
pierre11 | 4:707923a648dc | 103 | for (int pos = 2000; pos > 500; pos -= 25) |
pierre11 | 4:707923a648dc | 104 | { |
pierre11 | 3:9640475cdfbe | 105 | Servo2.SetPosition(pos); |
pierre11 | 3:9640475cdfbe | 106 | wait_ms(20); |
pierre11 | 4:707923a648dc | 107 | } |
pierre11 | 4:707923a648dc | 108 | } |
pierre11 | 4:707923a648dc | 109 | void move_3() |
pierre11 | 4:707923a648dc | 110 | { |
pierre11 | 4:707923a648dc | 111 | for (int pos = 500; pos < 2000; pos += 25) |
pierre11 | 3:9640475cdfbe | 112 | { |
pierre11 | 3:9640475cdfbe | 113 | Servo3.SetPosition(pos); |
pierre11 | 3:9640475cdfbe | 114 | wait_ms(20); |
pierre11 | 3:9640475cdfbe | 115 | } |
pierre11 | 4:707923a648dc | 116 | |
pierre11 | 4:707923a648dc | 117 | for (int pos = 2000; pos > 500; pos -= 25) |
pierre11 | 4:707923a648dc | 118 | { |
pierre11 | 3:9640475cdfbe | 119 | Servo3.SetPosition(pos); |
pierre11 | 3:9640475cdfbe | 120 | wait_ms(20); |
pierre11 | 4:707923a648dc | 121 | } |
pierre11 | 4:707923a648dc | 122 | } |
pierre11 | 4:707923a648dc | 123 | void move_4() |
pierre11 | 4:707923a648dc | 124 | { |
pierre11 | 4:707923a648dc | 125 | for (int pos = 500; pos < 2000; pos += 25) |
pierre11 | 4:707923a648dc | 126 | { |
pierre11 | 4:707923a648dc | 127 | Servo5.SetPosition(pos); |
pierre11 | 4:707923a648dc | 128 | wait_ms(20); |
pierre11 | 3:9640475cdfbe | 129 | } |
pierre11 | 3:9640475cdfbe | 130 | |
pierre11 | 4:707923a648dc | 131 | for (int pos = 2000; pos > 500; pos -= 25) |
pierre11 | 3:9640475cdfbe | 132 | { |
pierre11 | 4:707923a648dc | 133 | Servo5.SetPosition(pos); |
pierre11 | 3:9640475cdfbe | 134 | wait_ms(20); |
pierre11 | 4:707923a648dc | 135 | } |
pierre11 | 4:707923a648dc | 136 | } |
pierre11 | 4:707923a648dc | 137 | |
pierre11 | 4:707923a648dc | 138 | int colour() |
pierre11 | 4:707923a648dc | 139 | { |
pierre11 | 4:707923a648dc | 140 | color.SetMode(TCS3200::SCALE_100); |
pierre11 | 4:707923a648dc | 141 | long red, green, blue, clear; |
pierre11 | 4:707923a648dc | 142 | int r = 0,g = 0, b =0 ; |
pierre11 | 4:707923a648dc | 143 | for(int c=0 ; c < 10; c++) |
pierre11 | 4:707923a648dc | 144 | { |
pierre11 | 4:707923a648dc | 145 | |
pierre11 | 4:707923a648dc | 146 | red = color.ReadRed(); |
pierre11 | 4:707923a648dc | 147 | green = color.ReadGreen(); |
pierre11 | 4:707923a648dc | 148 | blue = color.ReadBlue(); |
pierre11 | 4:707923a648dc | 149 | clear = color.ReadClear(); |
pierre11 | 4:707923a648dc | 150 | |
pierre11 | 4:707923a648dc | 151 | if((red < blue) && (red < green)) |
pierre11 | 4:707923a648dc | 152 | { |
pierre11 | 4:707923a648dc | 153 | r++; // red |
pierre11 | 4:707923a648dc | 154 | } |
pierre11 | 4:707923a648dc | 155 | if((green < blue) && (green < red)) |
pierre11 | 4:707923a648dc | 156 | { |
pierre11 | 4:707923a648dc | 157 | g++;//green |
pierre11 | 4:707923a648dc | 158 | } |
pierre11 | 4:707923a648dc | 159 | if(blue < red && blue < green) |
pierre11 | 4:707923a648dc | 160 | { |
pierre11 | 4:707923a648dc | 161 | b++;//blue |
pierre11 | 4:707923a648dc | 162 | } |
pierre11 | 4:707923a648dc | 163 | |
pierre11 | 4:707923a648dc | 164 | } |
pierre11 | 4:707923a648dc | 165 | |
pierre11 | 4:707923a648dc | 166 | if((r > b) && (r > g)) |
pierre11 | 4:707923a648dc | 167 | { |
pierre11 | 4:707923a648dc | 168 | return 1; //red |
pierre11 | 4:707923a648dc | 169 | } |
pierre11 | 4:707923a648dc | 170 | else |
pierre11 | 4:707923a648dc | 171 | { |
pierre11 | 4:707923a648dc | 172 | if(g > b && g > r) |
pierre11 | 4:707923a648dc | 173 | { |
pierre11 | 4:707923a648dc | 174 | return 2; //green |
pierre11 | 4:707923a648dc | 175 | } |
pierre11 | 4:707923a648dc | 176 | else |
pierre11 | 4:707923a648dc | 177 | { |
pierre11 | 4:707923a648dc | 178 | //blue |
pierre11 | 4:707923a648dc | 179 | return 3; |
pierre11 | 4:707923a648dc | 180 | } |
pierre11 | 4:707923a648dc | 181 | |
pierre11 | 4:707923a648dc | 182 | |
pierre11 | 4:707923a648dc | 183 | } |
pierre11 | 4:707923a648dc | 184 | |
pierre11 | 4:707923a648dc | 185 | } |
pierre11 | 4:707923a648dc | 186 | |
pierre11 | 4:707923a648dc | 187 |