Code: #pragma config(Sensor, S1, LEFTS, sensorTouch) #pragma config(Sensor, S3, RIGHTS, sensorTouch) #pragma config(Sensor, S4, COLOR, sensorCOLORFULL) #pragma config(Motor, motorB, CARRIAGE, tmotorNXT, PIDControl, encoder) #pragma config(Motor, motorA, PUSHER, tmotorNXT, PIDControl, encoder) #pragma config(Motor, motorC, BALLS, tmotorNXT, PIDControl, encoder) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// Sorts colored balls // UK 2.2.2013
// global variables int VaunuVali; int VaunuLukema; int vauhti = 20; string teksti; bool jatkuu = false; int vari;
void liikkuu(int nopeus) { motor[CARRIAGE] = nopeus; }
void seis() { motor[CARRIAGE] = 0; }
task main() { wait10Msec(100); // texts on startup teksti = ""; for(int i = 0; i <= 4; i++) { nxtDisplayTextLine(i, "%s", teksti); } teksti = "left <- pusher"; nxtDisplayTextLine(1, "%s", teksti); teksti = "right -> ball"; nxtDisplayTextLine(2, "%s", teksti); teksti = "orange -> start"; nxtDisplayTextLine(3, "%s", teksti); // setting ball feeder and pusher in right positions while (jatkuu != true) { while (nNxtButtonPressed == 1) { motor[BALLS] = -10; } motor[BALLS] = 0; while (nNxtButtonPressed == 2) { motor[PUSHER] = 10; } motor[PUSHER] = 0; if (nNxtButtonPressed == 3) { jatkuu = true; } }
// measure the distance between touch sensors (VaunuVali) // go first to the left then to the right liikkuu(vauhti); while (SensorValue(LEFTS) == 0) { } seis(); VaunuVali = 0; nMotorEncoder[CARRIAGE] = 0; liikkuu(-vauhti); while (SensorValue(RIGHTS) == 0) { } seis(); VaunuVali = abs(nMotorEncoder[CARRIAGE]);
jatkuu = false; while (jatkuu != true) {
// drop one ball to the pusher nMotorEncoder[BALLS] = 0; nMotorEncoderTarget[BALLS] = 180; motor[BALLS] = -5; while (nMotorRunState[BALLS] != runStateIdle) { } motor[BALLS] = 0; nMotorEncoder[BALLS] = 0; nMotorEncoderTarget[BALLS] = 180; motor[BALLS] = 10; while (nMotorRunState[BALLS] != runStateIdle) { } motor[BALLS] = 0;
// carriage to left and push ball slightly forward liikkuu(vauhti); while (SensorValue(LEFTS) == 0) { } seis(); nMotorEncoder[PUSHER] = 0; nMotorEncoderTarget[PUSHER] = 70; // slightly forward motor[PUSHER] = 10; while (nMotorRunState[PUSHER] != runStateIdle) { } motor[PUSHER] = 0;
// find the color and go to the right box vari = SensorValue(COLOR);
switch (vari) { case BLACKCOLOR: // black means no ball jatkuu = true; // end sorting VaunuLukema = VaunuVali / 2; break; case BLUECOLOR: VaunuLukema = 0; // blue in first box from left break; case GREENCOLOR: VaunuLukema = VaunuVali / 4; // green to second box break; case YELLOWCOLOR: VaunuLukema = (VaunuVali * 3) / 4; // yellow to third box break; case REDCOLOR: VaunuLukema = VaunuVali; // red to the last box break; default: VaunuLukema = VaunuVali; // unknown color also to last box }
nMotorEncoder[CARRIAGE] = 0; nMotorEncoderTarget[CARRIAGE] = VaunuLukema; if (VaunuLukema > 0) { liikkuu(-vauhti); while (nMotorRunState[CARRIAGE] != runStateIdle) { } seis(); }
// drop the ball if (jatkuu == false) { nMotorEncoder[PUSHER] = 0; nMotorEncoderTarget[PUSHER] = 290; motor[PUSHER] = 10; while (nMotorRunState[PUSHER] != runStateIdle) { } motor[PUSHER] = 0; wait10Msec(20); liikkuu(-vauhti); while (SensorValue(RIGHTS) == 0) { } seis(); } }
// exit the program PlaySound(soundBeepBeep); StopAllTasks(); } |