Raj Patel
Skittle Sorter Running Code.
Dependencies: Motor Servo mbed
Diff: main.cpp
- Revision:
- 0:2df76cd30860
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Apr 29 18:23:18 2016 +0000
@@ -0,0 +1,621 @@
+#include <math.h>
+#include <stdio.h>
+#include "mbed.h"
+#define commonAnode true
+#include <algorithm>
+#include "Servo.h"
+
+
+//Color Sensor
+I2C i2c(p9, p10); //pins for I2C communication (SDA, SCL)
+int sensor_addr = 41 << 1;
+DigitalOut led(p11);
+
+//IR
+Serial pc(USBTX, USBRX);
+Serial device(p13, p14); // tx, rx
+PwmOut IRLED(p21);
+
+//Solenoid
+DigitalOut Ctrl(p8); //Solenoid output control bit
+
+
+//Motor A
+PwmOut speedA(p23); // PWM Speed pin for Motor A
+DigitalOut fwdA(p17); // Digital out signal for forward direction for Motor A
+DigitalOut revA(p18); // Digital out signal for reverse direction for Motor A
+
+//MotorB
+PwmOut speedB(p24); // PWM Speed pin for Motor B
+DigitalOut fwdB(p20); // Digital out signal for forward direction for Motor B
+DigitalOut revB(p19); // Digital out signal for reverse direction for Motor B
+
+
+//Bottom Encoder
+DigitalIn Enc1(p16); //right motor
+int oldEnc1 = 0; //Was the encoder previously a 1 or zero?
+int ticks = 0;
+int ticksPrime = 0;
+int dist2move = 0;
+
+//Top Encoder
+DigitalIn Enc2(p15); //right motor
+int oldEnc2 = 0; //Was the encoder previously a 1 or zero?
+int ticks2 = 0;
+
+
+//Servo
+Servo servo(p25);
+
+//Prototypes (headers)
+void irSetup();
+void colorSensorSetup();
+int readColor();
+int readIR();
+void solOn();
+void solOff();
+void motorA(int);
+void motorB(int);
+void encCount(int);
+void encCount2(int);
+void moveBottom(int);
+void moveTop(int);
+void pulseBack(int);
+
+int redpos = 0;
+int greenpos = 0;
+int yellowpos = 0;
+int purplepos = 0;
+int randompos = 0;
+
+int turnAmount = 75;
+
+int greenList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int redList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int yellowList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int purpleList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+int garbageList[5] = {0,turnAmount*1,turnAmount*2,turnAmount*3,turnAmount*4+10};
+
+int currentColor = 4;
+int counter = 0;
+
+int main() {
+
+
+
+ //Color Sensor setup
+ colorSensorSetup();
+
+ //Bottom Encoder setup
+ Enc1.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+ //Top Encoder setup
+ Enc2.mode(PullUp); // requires a pullup resistor so i just used embed's feature.
+ servo = 0.05;
+ bool succ = true;
+ double servoClosed = -0.05;
+ while(1) {
+ //pc.printf("%d\r\n",ticks);
+ /*
+ readColor();
+ wait(1);
+
+
+
+ motorA(1);
+ wait(1);
+ motorA(0);
+ wait(1);
+
+
+ motorB(-1);
+ wait(1);
+ motorB(0);
+ wait(1);
+ motorB(1);
+ wait(1);
+ motorB(0);
+ wait(1);
+ */
+
+ /*
+ motorA(1);
+ if (readIR == 1){
+ motorA(0);
+ solOn();
+ wait(1);
+ int color = readColor();
+
+ if (color == 0){
+
+ }else if (color == 1){
+
+ }else if (color == 2){
+
+ }else if (color == 3){
+
+ }else{
+
+
+ }
+
+ solOff();
+ wait(0.5);
+ }
+
+ */
+
+ /*
+ motorB(1);
+ encCount(1);
+ if (ticks==1000){
+ motorB(0);
+ break;
+ }
+ motorA(1);
+ */
+
+ //succ = false;
+ if (succ){
+ moveTop(94);
+ succ = false;
+ wait(0.5);
+ }
+ encCount2(1);
+ //pc.printf("%d\r\n",ticks2);
+ wait(0.8);
+ int nextColor = readColor();
+ //pc.printf("%d,%d\n\r",currentColor,nextColor);
+ switch (currentColor){
+
+ case 0:
+
+ switch (nextColor){
+
+ case 0:
+
+ moveBottom(greenList[0]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+
+ break;
+ case 1:
+ moveBottom(greenList[1]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 2:
+ moveBottom(greenList[2]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 3:
+ moveBottom(greenList[3]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 4:
+ moveBottom(greenList[4]);
+ servo = servoClosed;
+ counter++;
+ break;
+ }
+ break;
+ case 1:
+
+ switch (nextColor){
+
+ case 0:
+
+ moveBottom(redList[4]);
+ servo = 0.4;
+ wait(1);
+ servo = 0.05;
+ counter++;
+ break;
+ case 1:
+ moveBottom(redList[0]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+
+ break;
+ case 2:
+ moveBottom(redList[1]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 3:
+ moveBottom(redList[2]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 4:
+ moveBottom(redList[3]);
+ servo = servoClosed;
+ counter++;
+ break;
+ }
+ break;
+ case 2:
+
+ switch (nextColor){
+
+ case 0:
+
+ moveBottom(yellowList[3]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 1:
+ moveBottom(yellowList[4]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 2:
+ moveBottom(yellowList[0]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+
+ break;
+ case 3:
+ moveBottom(yellowList[1]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 4:
+ moveBottom(yellowList[2]);
+ servo = servoClosed;
+ counter++;
+ break;
+ }
+ break;
+ case 3:
+
+ switch (nextColor){
+
+ case 0:
+
+ moveBottom(purpleList[2]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 1:
+ moveBottom(purpleList[3]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 2:
+ moveBottom(purpleList[4]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ counter++;
+ break;
+ case 3:
+ moveBottom(purpleList[0]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+
+ break;
+ case 4:
+ moveBottom(purpleList[1]);
+ servo = servoClosed;
+ counter++;
+ break;
+ }
+ break;
+ case 4:
+
+ switch (nextColor){
+
+ case 0:
+ ticksPrime = ticks;
+ dist2move = nextColor*75;
+ moveBottom(garbageList[1]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ if(ticks <= ticksPrime + dist2move){
+ succ = true;
+ }
+ counter++;
+ break;
+ case 1:
+ ticksPrime = ticks;
+ dist2move = nextColor*75;
+ moveBottom(garbageList[2]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ if(ticks <= ticksPrime + dist2move){
+ succ = true;
+ }
+ counter++;
+ break;
+ case 2:
+ ticksPrime = ticks;
+ dist2move = nextColor*75;
+ moveBottom(garbageList[3]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ if(ticks <= ticksPrime + dist2move){
+ succ = true;
+ }
+ counter++;
+ break;
+ case 3:
+ ticksPrime = ticks;
+ dist2move = nextColor*75;
+ moveBottom(garbageList[4]);
+ servo = 0.4;
+ wait(1);
+ servo = servoClosed;
+ if(ticks <= ticksPrime + dist2move){
+ succ = true;
+ }
+ counter++;
+ break;
+ case 4:
+ ticksPrime = ticks;
+ dist2move = nextColor*75;
+ moveBottom(garbageList[0]);
+ servo = servoClosed;
+ if(ticks <= ticksPrime + dist2move){
+ succ = true;
+ }
+ break;
+ }
+ break;
+ }
+ encCount(1);
+ currentColor = nextColor;
+ if (counter==4){
+ pulseBack(16);
+ counter = 0;
+ pc.printf("%d\r\n",counter);
+ }
+
+ }
+}
+
+int readColor(){
+
+
+ // Initialize color sensor
+ char clear_reg[1] = {148};
+ char clear_data[2] = {0,0};
+ i2c.write(sensor_addr,clear_reg,1, true);
+ i2c.read(sensor_addr,clear_data,2, false);
+
+ int clear_value = ((int)clear_data[1] << 8) | clear_data[0];
+
+ char red_reg[1] = {150};
+ char red_data[2] = {0,0};
+ i2c.write(sensor_addr,red_reg,1, true);
+ i2c.read(sensor_addr,red_data,2, false);
+
+ int red_value = ((int)red_data[1] << 8) | red_data[0];
+
+ char green_reg[1] = {152};
+ char green_data[2] = {0,0};
+ i2c.write(sensor_addr,green_reg,1, true);
+ i2c.read(sensor_addr,green_data,2, false);
+
+ int green_value = ((int)green_data[1] << 8) | green_data[0];
+
+ char blue_reg[1] = {154};
+ char blue_data[2] = {0,0};
+ i2c.write(sensor_addr,blue_reg,1, true);
+ i2c.read(sensor_addr,blue_data,2, false);
+
+ int blue_value = ((int)blue_data[1] << 8) | blue_data[0];
+
+ // print sensor readings
+
+
+ //pc.printf("Clear (%d), Red (%d), Green (%d), Blue (%d)\n\r", clear_value, red_value, green_value, blue_value);
+ //pc.printf("Red (%d), Green (%d), Blue (%d)\n\r", red_value, green_value, blue_value);
+
+
+ float red_in = (float)red_value;
+ float green_in = (float)green_value;
+ float blue_in = (float)blue_value;
+
+ float redSkittle[] = {4656.25,588.25,826};
+ float greenSkittle[] = {2968.25,3898.5,1346.5};
+ float yellowSkittle[] = {15387.75,9977.5,3173.75};
+ //float purpleSkittle[] = {1088,342.25,340.5};
+ float purpleSkittle[] = {1215,1773,2541};
+ float closed[] = {1094, 1149, 987};
+
+ float distRed = sqrt(pow(red_in-redSkittle[0],2)+pow(green_in-redSkittle[1],2)+pow(blue_in-redSkittle[2],2));
+ float distGreen = sqrt(pow(red_in-greenSkittle[0],2)+pow(green_in-greenSkittle[1],2)+pow(blue_in-greenSkittle[2],2));
+ float distYellow = sqrt(pow(red_in-yellowSkittle[0],2)+pow(green_in-yellowSkittle[1],2)+pow(blue_in-yellowSkittle[2],2));
+ float distPurple = sqrt(pow(red_in-purpleSkittle[0],2)+pow(green_in-purpleSkittle[1],2)+pow(blue_in-purpleSkittle[2],2));
+ float distClosed = sqrt(pow(red_in-closed[0],2)+pow(green_in-closed[1],2)+pow(blue_in-closed[2],2));
+
+ float choices[] = {distRed, distGreen, distYellow, distPurple, distClosed};
+
+ sort(choices,choices+5);
+
+ float closest = choices[0];
+
+
+ if (closest==distRed){
+ //pc.printf("Red\n\r");
+ return 1;
+ }else if(closest==distGreen){
+ //pc.printf("Green\n\r");
+ return 0;
+ }else if(closest==distYellow){
+ //pc.printf("Yellow\n\r");
+ return 2;
+ }else if(closest==distPurple){
+ //pc.printf("Purple\n\r");
+ return 3;
+ }else //pc.printf("Dave messed up.\n\r");
+
+ return 4;
+
+
+}
+
+int readIR(){
+
+ if(pc.readable()) {
+ device.putc(pc.getc());
+ }
+ //IR Receive code
+ if(device.readable()) {
+ pc.putc(device.getc());
+ }
+ return 1;
+}
+
+void solOn(){
+ Ctrl = 1; //ON
+}
+
+void solOff(){
+ Ctrl = 0; //ON
+}
+
+void motorA(int state){
+ fwdA = 1;
+ revA = 0;
+ speedA = (float)state*0.15;
+}
+
+void motorB(int dir){
+ if (dir > 0){
+ fwdB = 1;
+ revB = 0;
+ }else{
+ fwdB = 0;
+ revB = 1;
+ dir = -dir;
+ }
+ speedB = (float)dir*.3;
+}
+void pulseBack(int dir) {
+ //int newticks = ticks - dir;
+ motorB(1);
+ /*
+ while (ticks > newticks){
+ encCount(-1);
+ //pc.printf("%d,%d\n\r",newticks,ticks2);
+ }
+ */
+ wait(0.1);
+ motorB(0);
+ return;
+}
+void irSetup(){
+ //IR Transmit setup code
+ IRLED.period(1.0/38000.0);
+ IRLED = 0.5;
+ //Drive IR LED data pin with 38Khz PWM Carrier
+ //Drive IR LED gnd pin with serial TX
+ device.baud(2400);
+}
+
+void colorSensorSetup(){
+ pc.baud(9600);
+
+ led = 1; // off
+ // Connect to the Color sensor and verify whether we connected to the correct sensor.
+
+ i2c.frequency(200000);
+
+ char id_regval[1] = {146};
+ char data[1] = {0};
+ i2c.write(sensor_addr,id_regval,1, true);
+ i2c.read(sensor_addr,data,1,false);
+
+ if (data[0]==68) {
+ //green = 0;
+ wait (2);
+ //green = 1;
+ } else {
+ //green = 1;
+ }
+
+ char timing_register[2] = {129,0};
+ i2c.write(sensor_addr,timing_register,2,false);
+
+ char control_register[2] = {143,0};
+ i2c.write(sensor_addr,control_register,2,false);
+
+ char enable_register[2] = {128,3};
+ i2c.write(sensor_addr,enable_register,2,false);
+
+
+}
+
+void encCount(int dir){
+ if(Enc1 != oldEnc1) { // Increment ticks every time the state has switched.
+ if (dir>0) ticks++;
+ else ticks--;
+ oldEnc1 = Enc1;
+ //pc.printf("%d\r\n",ticks);
+
+ }
+
+}
+
+
+void encCount2(int dir){
+ if(Enc2 != oldEnc2) { // Increment ticks every time the state has switched.
+ if (dir>0) ticks2++;
+ else ticks2--;
+ oldEnc2 = Enc2;
+ //pc.printf("%d\r\n",ticks2);
+
+ }
+
+}
+
+void moveBottom(int amt){
+ int newticks = ticks + amt;
+ motorB(-1);
+ while (ticks < newticks-5){
+ encCount(1);
+ //pc.printf("%d,%d\n\r",newticks,ticks);
+ }
+ motorB(0);
+ return;
+}
+void moveTop(int amt){
+ int newticks = ticks2 + amt;
+ motorA(1);
+ while (ticks2 < newticks-5){
+ encCount2(1);
+ //pc.printf("%d,%d\n\r",newticks,ticks2);
+ }
+ motorA(0);
+ return;
+}
\ No newline at end of file