pierre masala / Mbed 2 deprecated Servo

nmbbghgj

Dependencies:   mbed

Fork of Servo by Jasper Denkers

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?

UserRevisionLine numberNew 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