Hey! I'm back! With that Super-Top-Secret project I mentioned! I'll be posting more information about it over the next couple of days because I don't have time to do it all at once. Everyone fine with that Good.
I haven't taken any pics yet, so I won't be posting anything else about this until I do.
Here's the video to tide you over until then.
And again, I apologize for the video quality. Still testing out a new-ish camera, and I didn't really plan the video. I just did whatever came to mind while taping (SD-Carding?) it.
program:
Code: Select all
#pragma config(Sensor, S1, LightSens, sensorLightInactive)
#pragma config(Sensor, S2, ArmRst, sensorReflection)
#pragma config(Sensor, S3, ColorSens, sensorCOLORFULL)
#pragma config(Sensor, S4, HTIRL, sensorI2CCustom)
#pragma config(Motor, motorA, TmblrMtrSwtch, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorB, ArmMoveMtr, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorC, ArmTurnMtr, tmotorNormal, PIDControl, encoder)
#include "drivers/HTIRL-driver.h";
void initialize();
void InitializePF();
void ResetArmRot();
void ArmRotateToLoc(int DegreesToTurn);
void ArmLateralRst();
void ArmMoveToMarker(string DirectionToMove);
void ArmMoveToLoc(int LocToMoveTo);
void PneumSwtchMve(string ClawMovement);
void DispenseBrick4Scan();
void AlignBrick4Scan();
void TmblrMtrContr(string Direction);
void GetBlackOrGreenColor();
void RefillBrickQueue();
int BrickColor;
tPFmotor Tread1 = pfmotor_S4_C1_A;
tPFmotor Tread2AndLights = pfmotor_S4_C1_B;
tPFmotor DispenserMtr = pfmotor_S4_C2_A;
tPFmotor PneumSwtch = pfmotor_S4_C2_B;
tPFmotor BrickScannerDisp = pfmotor_S4_C3_A;
tPFmotor ColorScanAlign = pfmotor_S4_C3_B;
ePWMMotorCommand Tread1Fwd = MOTOR_FWD_PWM_4;
ePWMMotorCommand Tread2AndLightsFwd = MOTOR_FWD_PWM_7;
ePWMMotorCommand DispenserMtrDisp = MOTOR_FWD_PWM_7;
ePWMMotorCommand PneumClawUp = MOTOR_REV_PWM_7;
ePWMMotorCommand PneumClawDwn = MOTOR_FWD_PWM_7;
ePWMMotorCommand BrickScannerDispExt = MOTOR_FWD_PWM_3;
ePWMMotorCommand BrickScannerDispRtr = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignExt = MOTOR_REV_PWM_3;
ePWMMotorCommand ColorScanAlignRtr = MOTOR_FWD_PWM_3;
task main()
{
bool DoneSorting = false;
initialize();
while(DoneSorting == false)
{
SetSensorType(ColorSens, sensorCOLORNONE);
PFMotor(Tread2AndLights, Tread2AndLightsFwd);
wait1Msec(400);
if(SensorValue(LightSens) > 10)
{
RefillBrickQueue();
}
PFMotor(Tread1, Tread1Fwd);
DispenseBrick4Scan();
while(SensorValue(ColorSens) > 10);
PFMotor(Tread2AndLights, MOTOR_BRAKE);
PFMotor(Tread1, MOTOR_BRAKE);
SetSensorType(ColorSens, sensorCOLORFULL);
AlignBrick4Scan();
BrickColor = SensorValue(ColorSens);
if(BrickColor == 1 || BrickColor == 3) GetBlackOrGreenColor();
PFMotor(Tread2AndLights, Tread2AndLightsFwd);
wait1Msec(1000);
PFMotor(Tread2AndLights, MOTOR_BRAKE);
ArmMoveToLoc(BrickColor);
}
}
void initialize()
{
TmblrMtrContr("On");
TmblrMtrContr("Off");
InitializePF();
PneumSwtchMve("Up");
ArmLateralRst();
ResetArmRot();
PneumSwtchMve("Down");
ArmMoveToMarker("Left");
}
void InitializePF()
{
PFMotor(ColorScanAlign, ColorScanAlignRtr);
wait1Msec(500);
PFMotor(ColorScanAlign, MOTOR_BRAKE);
PFMotor(BrickScannerDisp, BrickScannerDispRtr);
wait1Msec(500);
PFMotor(BrickScannerDisp, MOTOR_BRAKE);
}
void ResetArmRot()
{
motor[ArmTurnMtr] = -50;
while(SensorValue(ArmRst) != 100);
motor[ArmTurnMtr] = 20;
while(SensorValue(ArmRst) == 100);
motor[ArmTurnMtr] = 0;
}
void ArmRotateToLoc(int DegreesToTurn)
{
nMotorEncoder[ArmTurnMtr] = 0;
nMotorEncoderTarget[ArmTurnMtr] = DegreesToTurn;
motor[ArmTurnMtr] = 50;
while(nMotorRunState[ArmTurnMtr] != runStateIdle);
motor[ArmTurnMtr] = 0;
}
void ArmLateralRst()
{
motor[ArmMoveMtr] = -30;
while(SensorValue(ArmRst) <= 49);
motor[ArmMoveMtr] = 0;
}
void ArmMoveToMarker(string DirectionToMove)
{
if(DirectionToMove == "Left")
{
motor[ArmMoveMtr] = 50;
wait1Msec(500);
while(SensorValue(ArmRst) < 40);
motor[ArmMoveMtr] = 0;
}
else
{
motor[ArmMoveMtr] = -50;
wait1Msec(500);
while(SensorValue(ArmRst) < 40);
motor[ArmMoveMtr] = 0;
}
}
void ArmMoveToLoc(int LocToMoveTo)
{
switch (LocToMoveTo)
{
case 1:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1132);
ArmMoveToMarker("Left");
PneumSwtchMve("Down");
wait1Msec(700);
ArmMoveToMarker("Right");
ResetArmRot();
break;
case 2:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1460);
ArmMoveToMarker("Left");
PneumSwtchMve("Down");
wait1Msec(700);
ArmMoveToMarker("Right");
ResetArmRot();
break;
case 3:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1132);
PneumSwtchMve("Down");
wait1Msec(700);
ResetArmRot();
break;
case 4:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1460);
PneumSwtchMve("Down");
wait1Msec(700);
ResetArmRot();
break;
case 5:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1132);
ArmMoveToMarker("Right");
PneumSwtchMve("Down");
wait1Msec(700);
ArmMoveToMarker("Left");
ResetArmRot();
break;
case 6:
PneumSwtchMve("Up");
wait1Msec(700);
ArmRotateToLoc(1460);
ArmMoveToMarker("Right");
PneumSwtchMve("Down");
wait1Msec(700);
ArmMoveToMarker("Left");
ResetArmRot();
break;
}
}
void PneumSwtchMve(string ClawMovement)
{
if(ClawMovement == "Up")
{
PFMotor(PneumSwtch, PneumClawUp);
wait1Msec(500);
PFMotor(PneumSwtch, MOTOR_BRAKE);
}
else
{
PFMotor(PneumSwtch, PneumClawDwn);
wait1Msec(500);
PFMotor(PneumSwtch, MOTOR_BRAKE);
}
}
void DispenseBrick4Scan()
{
PFMotor(BrickScannerDisp, BrickScannerDispExt);
wait1Msec(250);
PFMotor(BrickScannerDisp, BrickScannerDispRtr);
wait1Msec(250);
PFMotor(BrickScannerDisp, MOTOR_BRAKE);
}
void AlignBrick4Scan()
{
PFMotor(ColorScanAlign, ColorScanAlignExt);
wait1Msec(250);
PFMotor(ColorScanAlign, ColorScanAlignRtr);
wait1Msec(250);
PFMotor(ColorScanAlign, MOTOR_BRAKE);
}
void TmblrMtrContr(string Direction)
{
if(Direction == "On")
{
motor[TmblrMtrSwtch] = -50;
wait1Msec(500);
motor[TmblrMtrSwtch] = 0;
}
else
{
motor[TmblrMtrSwtch] = 50;
wait1Msec(500);
motor[TmblrMtrSwtch] = 0;
}
}
void GetBlackOrGreenColor()
{
short RGB[4];
getColorSensorData(ColorSens, colorAtoD, &RGB);
if((RGB[0]>=200&&RGB[0]<=300)&&(RGB[1]>=200&&RGB[1]<=300)&&(RGB[2]>=100&&RGB[2]<=199))
{
BrickColor = 1;
}
else if((RGB[0]>=400&&RGB[0]<=500)&&(RGB[1]>=400&&RGB[1]<=500)&&(RGB[2]>=200&&RGB[2]<=300))
{
BrickColor = 3;
}
}
void RefillBrickQueue()
{
bool DoneDispensing = false;
PFMotor(Tread1, Tread1Fwd);
TmblrMtrContr("On");
PFMotor(DispenserMtr, DispenserMtrDisp);
while(DoneDispensing == false)
{
if(SensorValue(LightSens) < 10)
{
wait1Msec(750);
if(SensorValue(LightSens) < 10) DoneDispensing = true;
}
}
TmblrMtrContr("Off");
PFMotor(DispenserMtr, MOTOR_BRAKE);
}
Comments, suggestions, LOLs?