pierre masala
/
Servo
nmbbghgj
Fork of Servo by
main.cpp
- Committer:
- pierre11
- Date:
- 2018-01-05
- Revision:
- 4:707923a648dc
- Parent:
- 3:9640475cdfbe
File content as of revision 4:707923a648dc:
#include "mbed.h" #include "Servo.h" #include "TCS3200.h" TCS3200 color(PF_6, PF_7, PF_8, PF_9, PA_0); //Create a TCS3200 object // S0 S1 S2 S3 OUT Servo Servo2(PD_12); Servo Servo3(PD_13); Servo Servo4(PD_14); Servo Servo5(PD_15); void move_1(); void move_2(); void move_3(); void move_4(); DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); int colour(); int main(int argc, char* argv[]) { //Set the scaling factor to 100% //color.SetMode(TCS3200::SCALE_100); myled1 = 0; myled2 = 0; myled3 = 0; Servo5.Enable(1500,20000); //turn Servo2.Enable(1500,20000); // forward Servo3.Enable(1500,20000); Servo4.Enable(1500,20000); // backward // while(1) { //1 move_1(); move_2(); move_3(); move_4(); if(colour()== 1) { printf("RED\n"); myled1 = 0; myled2 = 0; myled3 = 1; } else { if(colour() == 2) { printf("GREEN\n"); myled1 = 1; myled2 = 0; myled3 = 0; } else { printf("BLUE\n"); myled1 = 0; myled2 = 1; myled3 = 0; } } } return 0; } void move_1() { for (int pos = 500; pos < 2000; pos += 25) { Servo4.SetPosition(pos); wait_ms(20); } for (int pos = 2000; pos > 500; pos -= 25) { Servo4.SetPosition(pos); wait_ms(20); } } void move_2() { for (int pos = 500; pos < 2000; pos += 25) { Servo2.SetPosition(pos); wait_ms(20); } for (int pos = 2000; pos > 500; pos -= 25) { Servo2.SetPosition(pos); wait_ms(20); } } void move_3() { for (int pos = 500; pos < 2000; pos += 25) { Servo3.SetPosition(pos); wait_ms(20); } for (int pos = 2000; pos > 500; pos -= 25) { Servo3.SetPosition(pos); wait_ms(20); } } void move_4() { for (int pos = 500; pos < 2000; pos += 25) { Servo5.SetPosition(pos); wait_ms(20); } for (int pos = 2000; pos > 500; pos -= 25) { Servo5.SetPosition(pos); wait_ms(20); } } int colour() { color.SetMode(TCS3200::SCALE_100); long red, green, blue, clear; int r = 0,g = 0, b =0 ; for(int c=0 ; c < 10; c++) { red = color.ReadRed(); green = color.ReadGreen(); blue = color.ReadBlue(); clear = color.ReadClear(); if((red < blue) && (red < green)) { r++; // red } if((green < blue) && (green < red)) { g++;//green } if(blue < red && blue < green) { b++;//blue } } if((r > b) && (r > g)) { return 1; //red } else { if(g > b && g > r) { return 2; //green } else { //blue return 3; } } }