El escorpión - vex IQ

Uno de los proyectos más difíciles a los que me he enfrentado por ahora, es este escorpión, la dificultad viene dada por que el kit de vex IQ que tengo, no tiene suficientes piezas para hacerlo como me hubiese gustado y he tenido que improvisar para salir al paso en cada problema encontrado, pero bueno, el resultado es bastante aparente como veréis a continuación.

Hay que destacar que el sistema de movimiento se basa en dos motores que hay debajo del escorpión y que se mueven haciendo girar tres ruedas dentadas que cada una hace mover una pata, y el desfase de la posición de cada pata hace posible el movimiento.

Componentes:

• Distintas piezas de construcción
• 4 motores
• 1 sensor de distancia
• 1 gyro
• 1 touch led

En primer lugar decir que este escorpión con un aspecto que da un poco de miedo tiene las siguientes características:

• Se pone en marcha y se para al pulsar el touch led.
• Anda de forma autónoma, salvando los obstáculos que pueda encontrar a su paso, esto lo hace basándose en el sensor de distancia y girando 90º a la izquierda usando el gyro cada vez que detecta un obstáculo. 
• En caso de encontrar un objeto de color rojo suena una sirena, activa su potente aguijón para atacarlo y después retrocede un poco hacia atrás. 
• En caso de encontrarse un objecto verde, lo coje con sus pinzas y da por finalizada su misión con un sonido, avanzado para adelante un poco y mostrando un mensaje de 'Conseguido' en el display.

Como novedad en este desarrollo, es la utilización de RobotC vex IQ para la programación del mismo en vez de ModKit, no es que no se podría llevar a cavo usando ModKit, sino que ya quiero empezar a usar algo mas avanzado para los desarrollos futuros.

Sin mas aquí os pongo un vídeo que muestra las virtudes que ya hemos mencionado de este simpático a la vez que aterrador robot hecho íntegramente con vex IQ.

Como no podia faltar, os paso el código hecho con RobotC para vex IQ.

---

#pragma config(Sensor, port3,  colorSensor,    sensorVexIQ_ColorHue)
#pragma config(Sensor, port4,  distanceSensor, sensorVexIQ_Distance)
#pragma config(Sensor, port5,  gyroSensor,     sensorVexIQ_Gyro)
#pragma config(Sensor, port7,  touchLED,       sensorVexIQ_LED)
#pragma config(Motor,  motor1,          leftMotor,     tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor,  motor6,          rightMotor,    tmotorVexIQ, PIDControl, reversed, encoder)
#pragma config(Motor,  motor10,         colaMotor,   tmotorVexIQ, PIDControl, encoder)
#pragma config(Motor,  motor11,         pinzasMotor,     tmotorVexIQ, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool working;
TSimpleColors colorName;
bool greenFound;

void colorDetection()
{
if (colorName == colorRed && getColorProximity(colorSensor) > 280)
{
stopMultipleMotors(leftMotor, rightMotor);

int count = 0;
while(count < 3)
{
playSound(soundCarAlarm2);

setMotorSpeed(colaMotor, -20);
sleep(1000);
setMotorSpeed(colaMotor, 20);
sleep(1000);

count++;
}
stopMotor(colaMotor);

setMotorSpeed(leftMotor, -50);  //Move left motor at 50% speed
setMotorSpeed(rightMotor, -50); //Move right motor at 50% speed

// Se mueve 3 sg
sleep(3000);
stopMultipleMotors(leftMotor, rightMotor);
}
else if (colorName == colorGreen && getColorProximity(colorSensor) > 300)
{
stopMultipleMotors(leftMotor, rightMotor);

playSound(soundTada);

greenFound = true;

eraseDisplay();
displayCenteredBigTextLine(1, "Conseguido!");

setMotorSpeed(pinzasMotor, -25);
sleep(1000);
stopMotor(pinzasMotor);

setMotorSpeed(leftMotor, 50);
setMotorSpeed(rightMotor, 50);
sleep(4000);
stopAllMotors();
}
}

void refreshDisplay()
{
displayTextLine(0, "Distance: %d mm", getDistanceValue(distanceSensor));
displayTextLine(1, "working: %d", working);
displayTextLine(2, "Degrees: %d", getGyroDegrees(gyroSensor));

if (getColorProximity(colorSensor) > 200)
{
if(getColorValue(colorSensor) < 30)
{
colorName = colorRed;
displayTextLine(3, "Color: Red Dist: %d", getColorProximity(colorSensor));
}
if(getColorValue(colorSensor) >= 30 && (getColorValue(colorSensor) <= 50))
{
colorName = colorNone;
displayTextLine(3, "Color:");
}
else if((getColorValue(colorSensor) > 50) && (getColorValue(colorSensor) <= 130))
{
colorName = colorGreen;
displayTextLine(3, "Color: Green Dist: %d", getColorProximity(colorSensor));
}
else if((getColorValue(colorSensor) > 131) && (getColorValue(colorSensor) <= 219))
{
colorName = colorBlue;
displayTextLine(3, "Color: Blue Dist: %d", getColorProximity(colorSensor));
}
else if(getColorValue(colorSensor) > 219)
{
colorName = colorRed;
displayTextLine(3, "Color: Red Dist: %d", getColorProximity(colorSensor));
}
}
else
{
colorName = colorNone;
displayTextLine(3, "Color:");
}
}

task main()
{
// Valores por defecto
greenFound = false;
working = false;

resetMotorEncoder(leftMotor);
resetMotorEncoder(rightMotor);
resetMotorEncoder(pinzasMotor);
setMotorBrakeMode(pinzasMotor, motorHold);
resetMotorEncoder(colaMotor);
setMotorBrakeMode(colaMotor, motorHold);

//Set the minimum detectable range to 100mm (10cm/0.1m)
setDistanceMinRange(distanceSensor, 100);

//Set the maximum detectable range to 5000mm (50cm/0.5m)
setDistanceMaxRange(distanceSensor, 500);

// Lo dejo en amarillo parpadeando
setTouchLEDColor(touchLED, colorYellow);
setTouchLEDBlinkTime(touchLED, 40, 10);

while(!greenFound)
{
refreshDisplay();

if(getTouchLEDValue(touchLED) != 0)
{
working = !working;

if (working)
{
// Fin del parpadeo
setTouchLEDBlinkTime(touchLED, 0, 0);
}
else
{
// Que parpadee en amarillo
setTouchLEDColor(touchLED, colorYellow);
setTouchLEDBlinkTime(touchLED, 40, 10);

// Paro los motores
stopMultipleMotors(leftMotor, rightMotor);
}
}

if (working)
{
if (getDistanceValue(distanceSensor) > 400)
{
setTouchLEDColor(touchLED, colorGreen);

setMotorSpeed(leftMotor, 80);  //Move left motor at 80% speed
setMotorSpeed(rightMotor, 80); //Move right motor at 80% speed

// Se mueve 10 msg
sleep(10);
}
else if (getDistanceValue(distanceSensor) > 300)
{
setTouchLEDColor(touchLED, colorOrange);

setMotorSpeed(leftMotor, 50);  //Move left motor at 50% speed
setMotorSpeed(rightMotor, 50); //Move right motor at 50% speed

// Se mueve 10 msg
sleep(10);
}
else
{
setTouchLEDColor(touchLED, colorRed);

//Reset the gyro sensor to remove any previous data
resetGyro(gyroSensor);

//Loop while the gyro sees a value less than 90 degrees.
long angle = getGyroDegrees(gyroSensor) + 90;
while(getGyroDegrees(gyroSensor) < angle)
{
refreshDisplay();

//Set the motors to turn to the left at 25% speed.
setMotorSpeed(leftMotor, -40);
setMotorSpeed(rightMotor, 40);

colorDetection();
}
}

colorDetection();
}
}

sleep(5000); // Antes de finalizar el programa
}

---