//====================================================
// Task to handle the sonar sensors
//====================================================
task sonarSensors()
{
while(true)
{
//-------------------------
// Sonar
//-------------------------
num = USreadDist(LEGOUS);
num2 = USreadDist(LEGOUS2);
if(num != 255) sonarLive = num;
if(num2 != 255) sonarLive2 = num2;
}
}
float getUltraAngle()
{
int offset = 17;
int servoVal = 50 * 256 / 180 + offset;
int endPos = 120 * 256 / 180 + offset + 1;
servo[ultraTurret] = servoVal;
wait10Msec(100);
valAtMin = 0;
read = 0;
int count = 0;
while (servoVal < endPos)
{
servoVal++;
servo[ultraTurret] = servoVal;
read = USreadDist(ultraSonic);
writeDebugStreamLine("%f; %f", read, servoVal);
if (read < 100)
{
//minimum = read;
valAtMin = servoVal;
count++;
}
if (count == 10)
break;
wait10Msec(1);
}
return ((float)(valAtMin + offset) * 180.0 / 256.0) + asin((2.0*2.54) / read);
}
int ultrasound(tSensors us_sensor, int distance, int ultrasound_dist1, int ultrasound_dist3)
#endif
{
int s_val;
init_path();
add_segment(distance, 0, 45);
stop_path();
dead_reckon();
wait1Msec(1000);
#ifdef __HTSMUX_SUPPORT__
s_val = USreadDist(us_sensor);
#else
s_val = SensorValue[us_sensor];
#endif
if (s_val < ultrasound_dist3) {
return 3;
} else if (s_val < ultrasound_dist1) {
return 1;
} else {
return 2;
}
}
void Distkeep()
{
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;
// In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;
ClearTimer(T1);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 )
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
if (time100[T1] > 50 || nMotorEncoder[L_Motor] / COUNTS_PER_INCH > 40)
{
motor[L_Motor] = 0;
motor[R_Motor] = 0;
powerOff();
noOp();
alive();
StopAllTasks();
wait10Msec(55);
}
}
// Move forward based on encoders while avoiding collisions
// If the ultrasonic sensors see another robot, this will stop the robot until the
// Uses both the ultrasonic sensors in parallel.
// other robot moves away.
// distance: the number of degrees to elapse on the encoders
// power: the power level the motors will be driven at
void ForwardWithTwoSonar(int distance, int power)
{
nMotorEncoder[mLeft] = 0;
nMotorEncoder[mRight] = 0;
while((nMotorEncoder[mRight]) < distance)
{
int value = USreadDist(Ultra);
int value2 = USreadDist(Ultra2);
if((value>=255 || value<0) || (value2>=255 || value<0))
{
motor[mLeft] = power;
motor[mRight] = power;
}
}
motor[mLeft] = 0;
motor[mRight] = 0;
}
// Move forward based on time while avoiding collisions
// If the ultrasonic sensors see another robot, this will stop the robot until the
// Uses both the ultrasonic sensors in parallel.
// other robot moves away.
// time: the number of milliseconds to move for
// power: the power level the motors will be driven at
void ForwardWithTimeUS(int time, int power)
{
unsigned long endTime = nPgmTime + time;
while(nPgmTime < endTime)
{
int mLeftUS = USreadDist (Ultra);
int mRightUS = USreadDist (Ultra2);
if ((mLeftUS > 0 && mLeftUS < 255) || (mRightUS > 0 && mRightUS < 255))
{
motor[mRight] = 0;
motor[mLeft] = 0;
}
else
{
motor[mRight] = power;
motor[mLeft] = power;
}
}
motor[mRight] = 0;
motor[mLeft] = 0;
}
void liftUpAndDownAndTakeDumpAndForward(){
int sonicpos = USreadDist(sonicSensor);
while(USreadDist(sonicSensor)!= 0){//not sure what value is need to check
motor[fr] = 25;
motor[fl] = 25;
motor[br] = 25;
motor[bl] = 25;
}
motor[fr] = 0;
motor[fl] = 0;
motor[br] = 0;
motor[bl] = 0;
motor[liftA] = 100;
motor[liftB] = 100;
wait1Msec(3100);
motor[liftA] = 0;
motor[liftB] = 0;
takeDump();
motor[liftA] = -100;
motor[liftB] = -100;
wait1Msec(2900);
motor[liftA] = 0;
motor[liftB] = 0;
while(USreadDist(sonicSensor)!= sonicpos){
motor[fr] = -25;
motor[fl] = -25;
motor[br] = -25;
motor[bl] = -25;
}
motor[fr] = 0;
motor[fl] = 0;
motor[br] = 0;
motor[bl] = 0;
}
int statRead(tMUXSensor uc, int samples) {
//array of readings
int[samples] readings;
for(int i=0;i<samples;i++) {
readings[i]=USreadDist(uc);
wait1Msec(10);
}
//sort array
for (int i = 0, j = 0; i < samples; i+=((j<samples)?0:(j=i+1)))
if (readings[i] > readings[j]) {
int tmp = readings[i];
readings[i] = readings[j];
readings[j] = tmp;
}
return readings[samples/2];
}
task ball_watch()
{
int dist;
int i;
while (true) {
dist = USreadDist(LEGOUS);
if (dist <= 7) {
wait1Msec(500);
for (i = SERVO_ROLLER_OSC_UP; i <= SERVO_ROLLER_OSC_DOWN; i++) {
servo[roller] = i;
wait1Msec(10);
}
}
}
}
task main()
{
int i;
int center_position;
initialize_robot();
//waitForStart();
// startTask(raise_elbow);
startTask(raise_hand);
startTask(lower_ramp);
// initialize_receiver(irr_left, irr_right);
servo[leftEye] = LSERVO_CENTER;
servo[rightEye] = RSERVO_CENTER;
center_position = ultrasound_mk(carrot, -24, US_DIST_POS_1, US_DIST_POS_2, US_DIST_POS_3);
//startTask(raise_ramp);
if (center_position == -1) {
playImmediateTone(251, 150);
}
for (i = 0; i < center_position; i++) {
playImmediateTone(251, 50);
wait1Msec(1000);
}
nMotorEncoder[elbow] = 0;
motor[elbow] = 20;
while (nMotorEncoder[elbow] <= 3600) {
nxtDisplayCenteredBigTextLine(3, "%d", nMotorEncoder[elbow]);
}
motor[elbow] = 0;
// move_to_pole(center_position);
while(true) {
nxtDisplayCenteredBigTextLine(3, "%d", USreadDist(carrot));
}
}
task main()
{
int a = 0;
int j = 0;
int k = 0;
initializeRobot();
//waitForStart();
banana(false);
while(true){
sonarvalue = USreadDist(Sonar);
writeDebugStreamLine("sonar = %d", sonarvalue);
if(sonarvalue == 255){
//Diagonal center console
a++;
writeDebugStreamLine("a = %d", a);
if(a > 10){
writeDebugStreamLine("Diagonal, %d", sonarvalue);
autoDiag();
wait1Msec(2000);
break;
}
//The ultrasonic sensor cannot detect diagonal surfaces - therefore, it returns 255 as its default value.
}else if(abs(sonarvalue) < 118 && abs(sonarvalue) != 0){
//goal is straight ahead
j++;
writeDebugStreamLine("j = %d", j);
if(j > 5){
writeDebugStreamLine("Ahead, %d", sonarvalue);
autoStraight();
break;
}
}else if(abs(sonarvalue) >= 118){
//goal is sideways
k++;
writeDebugStreamLine("k = %d", k);
if(k > 5){
writeDebugStreamLine("Sideways, %d", sonarvalue);
autoHoriz();
break;
}
}
}
}
// This MUXing should really live up one layer of abstraction with two different instances underlying
// But this is RobotC and globals are the order of the day, so this is what we get instead
int readSonar() {
tMUXSensor active;
tMUXSensor inactive;
if(isLiftAboveRobot()) {
active = sonarLowDevice;
inactive = sonarHighDevice;
} else {
active = sonarHighDevice;
inactive = sonarLowDevice;
}
USsetOff((tSensors)SPORT(inactive));
USsetSingleMode((tSensors)SPORT(active));
wait1Msec(SONAR_DELAY);
int sonar = USreadDist(active);
if (sonar < SONAR_MIN || sonar > SONAR_MAX) {
sonar = 0;
}
#ifdef SONAR_DEBUG
nxtDisplayCenteredBigTextLine(1, "S: %d", sonar);
#endif
return sonar;
}
task displaySensorValues()
{
while(true)
{
IRLeftValue = HTIRS2readDCDir(IRLeft);
IRRightValue = HTIRS2readDCDir(IRRight);
gyroValue = HTGYROreadCal(gyro);
gyroValue2 = HTGYROreadRot(gyro);
ultrasoundValue = USreadDist(ultrasound);
eraseDisplay();
nxtDisplayString(0, "ir1: %d", IRLeftValue);
nxtDisplayString(2, "gyrocal: %d", gyroValue);
nxtDisplayString(3, "gyrorot: %d", gyroValue2);
nxtDisplayString(5, "ir2: %d", IRRightValue);
nxtDisplayString(7, "ultra: %d", ultrasoundValue);
Wait1Msec(2);
}
}
task main () {
int dist = 0;
nxtDisplayCenteredTextLine(0, "Lego");
nxtDisplayCenteredBigTextLine(1, "US");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and US sensor");
nxtDisplayCenteredTextLine(7, "to SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
nxtDisplayTextLine(0, "Lego US Sensor");
while(true) {
// Read the current distance detected.
dist = USreadDist(LEGOUS);
// display the info from the sensor
nxtDisplayTextLine(3, "Dist: %3d cm", dist);
wait10Msec(50);
}
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35; // In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;
wait10Msec(1500);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin)
{
nxtDisplayCenteredTextLine(1, "%d",USreadDist(SONAR_1));
nxtDisplayCenteredTextLine(2, "%d",SensorValue[IR]);
#if 1
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
#endif
}//End of while.
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
servo[Block_Chuck] = 255;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
while( USreadDist(SONAR_1) < 60)
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
}//End of while
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
Turn(65);
wait10Msec(55);
Move(25,100);
wait10Msec(55);
Turn(75);
wait10Msec(55);
Move(30 ,100);
wait10Msec(55);
servo[Block_Chuck] = 250;
wait10Msec(55);
servo[Spring_Release] = 250;
wait10Msec(55);
servo[Spring_Release] = 0;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
/* 1. We will need a sweep task.
2. Pathfinding move function.
A. Pythagorean theorum to find the sides
B. Try both sides of the obsticle if one side is obstructive.
C. 45 degrees.
*/
/*Scan = true;
while (Scan)
{
if (servo[IRS_1] == 0)
{
servo[IRS_1] = 255;
}
else if (servo[IRS_1] == 255)
{
servo[IRS_1] = 0;
}
}//End of while*/
} // End main task
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
clearDebugStream();
//Pos 1: 0 -> 0
//Pos 2: 0 -> 5
//Pos 3: 5
Move(-23, 0.6);
wait10Msec(100);
int irPos = 0;
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
{
writeDebugStreamLine("%d", HTIRS2readDCDir(ir));
if (HTIRS2readDCDir(ir) == 0)
irPos = 1;
else
irPos = 2;
}
else
irPos = 3;
writeDebugStreamLine("%d", irPos);
if (irPos == 3)
{
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
getUltraAngle(false);
Move(-15, 0.5);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(100);
//Move(-34, 0.6);
wait10Msec(1);
score();
}
else if (irPos == 2)
{
Turn(-60);
wait10Msec(1);
Move(-26, 0.4);
wait10Msec(1);
Turn(104);
wait10Msec(1);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
wait10Msec(1);
Turn(90 - getUltraAngle(false));
wait10Msec(1);
Move(-(((float)USreadDist(ultraSonic) / 2.54) - 9), 0.2);
wait10Msec(1);
score();
}
else
{
//leave zone
Move(13, 1);
wait10Msec(10);
Turn(28);
wait10Msec(10);
Move(-65, 1);
wait10Msec(10);
Turn(-23);
wait10Msec(10);
Move(-37, 0.9);
wait10Msec(10);
Move(-10, 0.2);
//Precision Line-Up
motor[lift] = 75;
bool raising = false;
int timeValue = 200;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//while(true){}
float Angle = 0;
Angle = getUltraAngle(true);
/*if (Angle == -1)
Move(-5, 0.3);
else if (Angle >= 86 && Angle <= 90)
break;*/
writeDebugStreamLine("%f", Angle);
Turn(Angle - 90);
float dist = -10;
Move(dist, 0.25);
//grab goal
wait10Msec(30);
servo[hook1] = 235;
servo[hook2] = 30;
wait10Msec(150);
Turn(90 - Angle);
wait10Msec(10);
Move(117, 1);
wait10Msec(10);
Turn(-100);
wait10Msec(10);
Move(-25, 0.5);
wait10Msec(10);
//score
motor[lift] = 75;
raising = true;
timeValue = 800;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
//Move(3, 0.2);
wait10Msec(10);
servo[output] = 250;
wait10Msec(500);
servo[output] = 130;
//release goal
servo[hook1] = 0;
servo[hook2] = 255;
wait10Msec(300);
Move(5, 1);
wait10Msec(30);
Turn(90);
}
if (irPos != 1)
{
//KickStand
wait10Msec(200);
servo[output] = 130;
motor[lift] = -75;
bool raising = false;
int timeValue = 4000;
ClearTimer(T3);
while (abs(time1[T3]) <= timeValue)
{
if (TSreadState(touch) != 1 && !raising)
{
ClearTimer(T3);
raising = true;
}
wait10Msec(10);
}
motor[lift] = 0;
Turn(90);
wait10Msec(1);
Move(-10, 0.5);
wait10Msec(1);
Turn(80);
wait10Msec(1);
Move(20, 0.9);
}
}
task main(){
calibrate();
while(true){
length = USreadDist(USBack);
rightWidth = USreadDist(USRight);
leftWidth = USreadDist(USLeft);
haveBall = TSreadState(HaveBaller);
frontIRValue = HTIRS2readACDir(IRFront);
currentRelCompass = HTMCreadRelativeHeading(Compass);
HTCS2readRGB(Colour, currentRed, currentGreen, currentBlue);
isWhite = (currentRed > whiteThreshold && currentGreen > whiteThreshold && currentBlue > whiteThreshold);
nxtDisplayTextLine(1, "Com: %4d", currentRelCompass);
nxtDisplayTextLine(2, "IR: %4d", frontIRValue);
nxtDisplayTextLine(3, "Len: %4d", length);
nxtDisplayTextLine(4, "RWid: %4d", rightWidth);
nxtDisplayTextLine(5, "LWid: %4d", leftWidth);
nxtDisplayTextLine(6, "Ball: %4d", haveBall);
if(isWhite){
nxtDisplayTextLine(7, "White: Yes");
}
else{
nxtDisplayTextLine(7, "White: No");
}
if(currentRelCompass < 0 - nbound){
move(CW, turnPower);
}
else if(currentRelCompass > 0 + nbound){
move(CC, turnPower);
}
else{
switch(frontIRValue){
case 0:
move(ST);
break;
case 1:
move(L);
break;
case 2:
move(L);
break;
case 3:
move(L);
break;
case 4:
move(L);
break;
case 5:
if(leftWidth < 45 && leftWidth != 0){
if(length < 20){
move(F);
}
else{
move(R);
}
}
else if(rightWidth < 45 && rightWidth != 0){
if(length < 20){
move(F);
}
else{
move(L);
}
}
else{
if (length > fbound){
move(B);
}
else if(length < 8){
move(F);
}
else{
move(ST);
}
}
break;
case 6:
move(R);
break;
case 7:
move(R);
break;
case 8:
move(R);
break;
case 9:
move(R);
break;
}
wait1Msec(50);
}
}
}
//========== Functions ==========
int sensorsUltrasonicGetDistance() {
return USreadDist(sonarMUX);
}
task sensors()
{
LSsetActive(LEGOLS);
int acS[5];
//-------------------------
// gyro
//-------------------------
long currtime,prevtime;
while (HTSMUXreadPowerStatus(S3)) // check battery power is on
{
PlayTone(750,25);
wait1Msec(500);
}
SMUX_good = true;
while(calibrate != 1){};
wait1Msec(300);
HTGYROstartCal(HTGYRO);
float drift = MyHTCal(gyroCalTime*1000);
//---------------------------------------
Delete(sFileName, nIoResult);
nFileSize = 100;
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize);
WriteFloat( hFileHandle, nIoResult, drift);
Close(hFileHandle, nIoResult);
//---------------------------------------
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (abs(highest-lowest)>10)
{
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2;
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
newgyro = (float)HTGYROreadRot(HTGYRO);
constHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (newgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
wait1Msec(1);
//-------------------------
// Sonar
//-------------------------
num = USreadDist(LEGOUS);
num2 = USreadDist(LEGOUS2);
if (num!=255)
{sonarLive = num;
}
if (num2!=255)
{ sonarLive2 = num2;
}
//if(num != 255) sonarLive = num;
//if(num2 != 255) sonarLive2 = num2;
//-------------------------
// light
//-------------------------
nrm = LSvalNorm(LEGOLS);
//-------------------------
// IR
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2);
#define max(a, b) (((a) > (b))? (a): (b))
#define min(a, b) (((a) < (b))? (a): (b))
currDir = (float) bearingAC;
if (bearingAC == 0)
{
currDir = prevDir;
}
else
{
if (HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]))
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
}
}
void kindOfAllOfAutonomous() // program to go to the IR beacon and drop the block and get on the bridge
{
float sL, sR; // strenght left and right
motor[leftWheel] = 30; // move forward
motor[rightWheel] = 30;
sL = strengthLeft(); // compute strengths of IR sensors
sR = strengthRight();
while(sL/sR<2.0 && sL/sR>0.5 ){ // if the strength are within a factor of 2 of each other
sL = strengthLeft();
sR = strengthRight();
}
if(sL>sR) // see which side the IR is...
{ // it is on the left!
while(HTIRS2readACDir(irLeft) != 5) // while we're not in front of the beacon.
{
sL = USreadDist(sonarLeft);
if(sL<50){
motor[leftWheel] = 30+ 0.5*(35-sL) ; // try to keep 20cm distance
motor[rightWheel] = 30- 0.5*(35-sL) ;
}
}
// we should now be in front of the beacon and the beacon is on the left
motor[slide] = -100;
wait1Msec(1500); //CALIBRATE
motor[slide] = 0;
turn(-90); // turn left
motor[leftWheel] = 15; // get closer to basket
motor[rightWheel] = 15;
wait1Msec(750); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[block] = -100; // spit it out...
wait1Msec(3000);
motor[block] = 0;
// retrace our steps...
motor[leftWheel] = -15;
motor[rightWheel] = -15;
wait1Msec(750); //CALIBRATE same as above
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[slide] = -100; // move our slide down
wait1Msec(1500);
motor[slide] = 0;
turn(90); //turn right
sL = USreadDist(sonarLeft);
while(sL<50){
motor[leftWheel] = -30- 0.5*(20-sL) ; // try to keep 20cm distance
motor[rightWheel] = -30+ 0.5*(20-sL) ;
sL = USreadDist(sonarLeft);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(-90); // turn left and go for the line...
motor[leftWheel] = 30; //move forward
motor[rightWheel] = 30;
float cs = 0; // find the line...
short nRawValues[4];
while(cs<3.5){
getColorSensorData(color, colorAtoD,&nRawValues[0]);
cs = (float)(nRawValues[0]+nRawValues[1]+nRawValues[2])/(3.0*nRawValues[3]);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(90); // turn right
motor[leftWheel] = 30;
motor[rightWheel] = 30;
wait1Msec(3000); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
else // the beacon is on the right
{
while(HTIRS2readACDir(irRight) != 5) // while we're not in front of the beacon.
{
sR = USreadDist(sonarRight);
if(sR<50){
motor[leftWheel] = 30- 0.5*(20-sR) ; // try to keep 20cm distance
motor[rightWheel] = 30+ 0.5*(20-sR) ;
}
}
// we should now be in front of the beacon and the beacon is on the right
motor[slide] = 100;
wait1Msec(1500); //CALIBRATE
motor[slide] = 0;
turn(90); // turn right
motor[leftWheel] = 15; // get closer to basket
motor[rightWheel] = 15;
wait1Msec(750); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[block] = -100; // spit it out...
wait1Msec(3000);
motor[block] = 0;
// retrace our steps...
motor[leftWheel] = -15;
motor[rightWheel] = -15;
wait1Msec(750); //CALIBRATE same as above
motor[leftWheel] = 0;
motor[rightWheel] = 0;
motor[slide] = -100; // move our slide down
wait1Msec(1500);
motor[slide] = 0;
turn(-90); //turn left
sR = USreadDist(sonarRight);
while(sR<50){
motor[leftWheel] = -30+ 0.5*(20-sR) ; // try to keep 20cm distance
motor[rightWheel] = -30- 0.5*(20-sR) ;
sR = USreadDist(sonarRight);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(90); // turn right and go for the line...
motor[leftWheel] = 30; //move forward
motor[rightWheel] = 30;
float cs = 0; // find the line...
short nRawValues[4];
while(cs<3.5){
getColorSensorData(color, colorAtoD,&nRawValues[0]);
cs = (float)(nRawValues[0]+nRawValues[1]+nRawValues[2])/(3.0*nRawValues[3]);
}
motor[leftWheel] = 0; // stop
motor[rightWheel] = 0;
turn(-90); // turn left
motor[leftWheel] = 30;
motor[rightWheel] = 30;
wait1Msec(3000); //CALIBRATE
motor[leftWheel] = 0;
motor[rightWheel] = 0;
}
//we are done...
}
//===================================================
// task to read in all sensors to workspace variables
//===================================================
task sensors()
{
float currDir = 0.0; //prevDir = 0.0,
long currtime,prevtime;
LSsetActive(LEGOLS); // set the LEGO light sensor to active mode
//-------------------------
// gyro
//-------------------------
ir_mux_status=HTSMUXreadPowerStatus(IR_MUX); // read the sensor multiplexor status
gyro_mux_status=HTSMUXreadPowerStatus(GYRO_MUX); // read the sensor multiplexor status
while (ir_mux_status || gyro_mux_status) // check good battery power on both muxes
{
PlayTone(750,25); // if not we beep indefinitely
wait1Msec(500);
}
//SMUX_good = true;
while(calibrate != 1){}; // wait for a request to start calibrating the gyro
wait1Msec(300); // short delay to ensure that user has released the button
HTGYROstartCal(HTGYRO); // initiate the GYRO calibration
drift = MyHTCal(gyroCalTime*1000);
Driver_Cal = HTGYROreadCal(HTGYRO); // read the calculated calibration value for saving to file
//---------------------------------------
// write the GYRO calibration data to file for Tele-Op
//---------------------------------------
Delete(sFileName, nIoResult); // delete any pre-existing file
nFileSize = 100; // new file size will be 100 bytes
OpenWrite( hFileHandle, nIoResult, sFileName, nFileSize); // create and open the new file
WriteFloat( hFileHandle, nIoResult, drift); // write the current drift value to the file
WriteFloat( hFileHandle, nIoResult, Driver_Cal); // write the driver calibration to the file
Close(hFileHandle, nIoResult); // close the file
//---------------------------------------
for (int i=0;i<5;i++) // check if there is too much spread in the data
{
if (gyro_noise>10) // if there is too much spread we beep 5 times to alert the drive team
{
gyroTrue = true;
PlayTone (250,25);
wait1Msec(500);
}
}
calibrate = 2; // this signifies to the main program that calibration has been completed
prevtime = nPgmTime;
while(true)
{
currtime=nPgmTime;
rawgyro = HTGYROreadRot(HTGYRO);
constHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
relHeading += (rawgyro - drift) * (float)(currtime-prevtime)/1000;
prevtime = currtime;
//wait1Msec(1);
//---------------------------------------------------------------------
// Read both sonar sensors and filter out non-valid echo readings (255)
// If there is no echo the filter just retains the last good reading
//---------------------------------------------------------------------
sonarRaw = USreadDist(LEGOUS); // read the rear mounted sensor
if (sonarRaw!=255) sonarLive = sonarRaw; // and copy valid results to workspace
sonarRaw2 = USreadDist(LEGOUS2); // read the side mounted sensor
if (sonarRaw2!=255) sonarLive2 = sonarRaw2; // and copy valid results to workspace
//-------------------------
// LEGO light sensor
//-------------------------
light_normalised = LSvalNorm(LEGOLS); // read the LEGO light sensor
//-------------------------
// HiTechnic IR Sensor
//-------------------------
bearingAC = HTIRS2readACDir(HTIRS2); // Read the IR bearing from the sensor
bearingAC2 = HTIRS2readACDir(HTIRS2_2);//here 12334
currDir = (float) bearingAC; // copy into workspace -
/*if (bearingAC == 0) // No IR signal is being detected
{
currDir = prevDir; // so retain the previous reading
}
else // otherwise read all the IR segments
{
{
bearingAC = (bearingAC - 1)/2;
if ((bearingAC < 4) && (acS[bearingAC] != 0) && (acS[bearingAC + 1] != 0))
{
currDir += (float)(acS[bearingAC + 1] - acS[bearingAC])/
max(acS[bearingAC], acS[bearingAC + 1]);
}
}
}
prevDir = currDir;
IR_Bearing=currDir-5; // and setup the main variable for others to use
*/
HTIRS2readAllACStrength(HTIRS2, acS[0], acS[1], acS[2], acS[3], acS[4]);
HTIRS2readAllACStrength(HTIRS2_2, acS2[0], acS2[1], acS2[2], acS2[3], acS2[4]);
//-----------------------------------
// code for the peaks of IR sensor 1
//-----------------------------------
if (bearingAC!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS[i]>maximum)
{peak = i;
maximum = acS[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS[peak] != 0)) // we are not working with extreme value
{
if (acS[peak-1]!=acS[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS[peak-1]>acS[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS[peak]-acS[peak-1])/ // calculate the bias away from the peak
max(acS[peak], acS[peak-1]));
}
else
{
offset = 25*(1-(float)(acS[peak]-acS[peak+1])/
max(acS[peak], acS[peak+1]));
}
}
}
IR_Bearing = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
//-----------------------------------
// code for the peaks of IR sensor 2
//-----------------------------------
if (bearingAC2!=0) // we have a valid IR signal
{
int maximum = -1;
int peak = 0, offset=0;
for (int i=0;i<5;i++) // scan array to find the peak entry
{ if (acS2[i]>maximum)
{peak = i;
maximum = acS2[i];
}
}
offset=0;
if ((peak < 4) && (peak>0) && (acS2[peak] != 0)) // we are not working with extreme value
{
if (acS2[peak-1]!=acS2[peak+1]) // if the values either side of the peak are identical then peak is peak
{
if (acS2[peak-1]>acS2[peak+1]) // otherwise decide which side has higher signal
{
offset = -25*(1-(float)(acS2[peak]-acS2[peak-1])/ // calculate the bias away from the peak
max(acS2[peak], acS2[peak-1]));
}
else
{
offset = 25*(1-(float)(acS2[peak]-acS2[peak+1])/
max(acS2[peak], acS2[peak+1]));
}
}
}
IR_Bearing2 = (float)((peak-2)*50) + offset; // direction is the total of the peak bias plus the adjacent bias
// range is -100 to +100, zero is straight ahead
}
}
}
task Foreground() {
//servoChangeRate[servo2] = 2;
Pid_Init1();
Pid_Init2();
int timeLeft;
int state=0;
int speedCmd = 0;
int dirCmd = 0;
servo[irArm]=105;
const tMUXSensor LEGOUS = msensor_S4_3;
while(true) {
ClearTimer(T1);
hogCPU();
//--------------------Robot Code---------------------------//
long armEncoder = nMotorEncoder[blockthrower];
long robotDist = nMotorEncoder[rtMotor] + nMotorEncoder[ltMotor];
long robotDir = nMotorEncoder[ltMotor] - nMotorEncoder[rtMotor];
long distInches = robotDist/IN2CLK;
long dirDegrees = robotDir/DEG2CLK;
// Calculate the speed and direction commands
if(shortPathTrue == false)
{
speedCmd = ForwardDist(path[pathIdx][DIST_IDX], robotDist, path[pathIdx][SPD_IDX]);
dirCmd=Direction(path[pathIdx][DIR_IDX], robotDir);
}
else
{
speedCmd = ForwardDist(shortPath[pathIdx][DIST_IDX], robotDist, shortPath[pathIdx][SPD_IDX]);
dirCmd=Direction(shortPath[pathIdx][DIR_IDX], robotDir);
}
int armSpd = FlipperArm(armEncoder, armSetPos);
bool IRval;
bool SonarVal;
//calculate when to increment path
if (abs(path[pathIdx][DIR_IDX] - dirDegrees) < 4 && abs(path[pathIdx][DIST_IDX] - distInches) < 3) pathIdx++;
// State O Follow Path
if (state==0)
{
if (distInches>18)
{
state=1;
}
}
IRval = Delayatrue(1, SensorValue[IR] == 4 || SensorValue[IR] == 3);
// State 1 Look for IR Beacon
if (state==1)
{
speedCmd=10;
if ( IRval)
{
state=7;
}
else
{
state=2;
}
}
// State 2 Follow Path
if (state==2)
{
if (distInches>27)
{
state=3;
}
}
// State 3 Look for IR Beacon
if (state==3)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=4;
}
}
// State 4 Follow Path
if (state==4)
{
if (distInches>46)//36
{
state=5;
}
}
// State 5 Look for IR Beacon
if (state==5)
{
speedCmd=10;
if ( IRval==true)
{
state=7;
}
else
{
state=6;
}
}
// State 6 Follow Path
if (state==6)
{
if (pathIdx == 1)//45
{
state=7;
}
}
if (state==7)// flip arm
{
speedCmd=0;
dirCmd = 0;
armSetPos = 1900;
if (abs(armSetPos - armEncoder) <20)
{
state=8;
}
servo[irArm]=243;
}
if (state==8)
{
speedCmd = 0;
dirCmd = 0;
armSetPos = 0;
if (abs(armSetPos) - abs(armEncoder) < 200)
{
state=9;
}
}
SonarVal = Delayatrue2(1, USreadDist(LEGOUS) > 40);
if (state==9)
{
if ((distInches>90) && (distInches<115))
{
if(SonarVal)
{
state=10;
}
else
{
state=11;
}
}
}
if(state==10)
{
shortPathTrue=true;
}
if(state==11)
{
}
/*
DebugInt("spd",speedCmd);
DebugInt("dir",robotDir/DEG2CLK);
DebugInt("sonarval",SonarVal);
DebugInt("path",pathIdx);
DebugInt("state",state);
DebugInt("dist",distInches);
DebugInt("ir", SensorValue[IR]);
*/
writeDebugStreamLine("i,pthIdx,rbtDist,irval,spdCmd,IR,state, rightencoder, leftencoder");
writeDebugStreamLine("%3i,%3i,%4i,%4i,%3i,%3i,%3i, %4i, %4i",iFrameCnt,pathIdx,distInches,IRval,speedCmd,SensorValue[IR],state, nMotorEncoder[rtMotor], nMotorEncoder[ltMotor]);
// Calculate when to move to the next path index
int s=sizeof(path)/sizeof(path[0])-1;
DebugInt("siz",s);
if (pathIdx>s) pathIdx=s;
speedCmd = RateLimit(speedCmd, START_RATE,speedCmdZ1 );
rightmotors=speedCmd-dirCmd;
leftmotors=speedCmd+dirCmd;
motor[rtBack]=rightmotors;
motor[rtMotor]=rightmotors;
motor[ltMotor]=leftmotors;
motor[ltBack]=leftmotors;
motor[blockthrower]=armSpd;
DebugInt("rightmotors",rightmotors);
DebugInt("leftmotors",leftmotors);
//--------------------------Robot Code--------------------------//
// Wait for next itteration
timeLeft=FOREGROUND_MS-time1[T1];
releaseCPU();
wait1Msec(timeLeft);
}// While
}//Foreground
#define fastMultiplier 2 /*how much faster to go in fast mode (1 = standard speed.)*/
#define slowMultiplier 0.5 /*how much slower to go in slow mode(1 = standard speed.)*/
#define backboardUp 250
#define backboardDown 15
#define grabberDown 100
#define grabberUp 140
int IRLeftValue = HTIRS2readDCDir(IRLeft);
int IRRightValue = HTIRS2readDCDir(IRRight);
float gyroValue = HTGYROreadCal(gyro);
float gyroValue2 = HTGYROreadRot(gyro);
int ultrasoundValue = USreadDist(ultrasound);
task displaySensorValues()
{
while(true)
{
IRLeftValue = HTIRS2readDCDir(IRLeft);
IRRightValue = HTIRS2readDCDir(IRRight);
gyroValue = HTGYROreadCal(gyro);
gyroValue2 = HTGYROreadRot(gyro);
ultrasoundValue = USreadDist(ultrasound);
eraseDisplay();
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
while(true)
{
bDisplayDiagnostics = false;// Wait for the beginning of autonomous phase.
eraseDisplay();
// WriteShort(hFileHandle, nIoResult, nParm);
if ( externalBatteryAvg < 0)
{
while (externalBatteryAvg < 0)
{
eraseDisplay();
nxtDisplayTextLine(1, "Ext Batt: OFF"); //External battery is off or not connected
nxtDisplayCenteredBigTextLine(1, "What!?");
nxtDisplayTextLine(3, "My name is,");
nxtDisplayTextLine(4, "Iego Montoya,");
nxtDisplayTextLine(5, "you did not turn");
nxtDisplayTextLine(6, "on the robot");
nxtDisplayTextLine(7, "NOW YOU DIE");
nxtDisplayTextLine(8, "YOU COCKROACH!!!!!!");
PlayImmediateTone(600, 20);
PlayImmediateTone(400, 20);
wait10Msec(20);
}
}
else
{
nxtDisplayTextLine(1, "Ext Batt:%4.1f V", externalBatteryAvg / (float) 1000);
}
while ( externalBatteryAvg / (float) 1000 < 13 && externalBatteryAvg / (float) 1000 > 0 || nAvgBatteryLevel / (float) 1000 < 7)
{
eraseDisplay();
nxtDisplayBigTextLine(6, "Battery");
nxtScrollText("poopheads");
nxtScrollText("You didnt change the battery you shmuts!");
nxtScrollText("the battery you");
nxtScrollText("shmuts!");
PlayImmediateTone(600, 70);
PlayImmediateTone(400, 70);
wait10Msec(20);
}
nxtDisplayTextLine(2, "NXT Batt:%4.1f V", nAvgBatteryLevel / (float) 1000); // Display NXT Battery Voltage
nxtDisplayTextLine(3, "R = %d L = %d", nMotorEncoder[R_Motor], nMotorEncoder[L_Motor]);
nxtDisplayTextLine(4, "SONAR_1 = %d", USreadDist(SONAR_1));
nxtDisplayTextLine(5, "IR = %d", SensorValue[IR]);
nxtDisplayTextLine(6, "Gyro = %d", SensorValue[Gyro]);
wait10Msec(20);
}//End of While
//Actuation tests.
motor[R_Motor] = 100;
wait10Msec(30);
motor[R_Motor] = 0;
wait10Msec(30);
motor[L_Motor] = 100;
wait10Msec(30);
motor[L_Motor] = 0;
wait10Msec(30);
motor[Flag_Raiser] = 100;
wait10Msec(30);
motor[Flag_Raiser] = 0;
wait10Msec(20);
motor[Winch] = 100;
wait10Msec(30);
motor[Winch] = 0;
wait10Msec(30);
motor[Arm] = 100;
wait10Msec(30);
motor[Arm] = 0;
wait10Msec(30);
motor[Block_Sweep] = 100;
wait10Msec(30);
motor[Block_Sweep] = 0;
wait10Msec(30);
servo[Bucket_Release] = 100;
wait10Msec(30);
servo[Bucket_Release] = 0;
wait10Msec(30);
servo[Spring_Release] = 100;
wait10Msec(30);
servo[Spring_Release] = 0;
wait10Msec(30);
servo[IRS_1] = 100;
wait10Msec(30);
servo[IRS_1] = 0;
wait10Msec(30);
}
task main()
{
initializeRobot();
waitForStart(); // Wait for the beginning of autonomous phase.
StartTask( Zelda_Theme );
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;
// In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;
ClearTimer(T1);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 )
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
}//End of while.
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
servo[Block_Chuck] = 255;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
while( USreadDist(SONAR_1) < 60)
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
/* if (time100[T1] > 60 || nMotorEncoder[L_Motor] / COUNTS_PER_INCH > 40)
{
motor[L_Motor] = 0;
motor[R_Motor] = 0;
noOp();
alive();
StopAllTasks();
wait10Msec(55);
}*/
}//End of while
motor[L_Motor] = 0;
motor[R_Motor] = 0;
wait10Msec(55);
Turn(65);
wait10Msec(55);
Move(25,100);
wait10Msec(55);
Turn(75);
wait10Msec(55);
Move(30 ,100);
wait10Msec(55);
servo[Block_Chuck] = 250;
wait10Msec(55);
servo[Spring_Release] = 250;
wait10Msec(55);
servo[Spring_Release] = 0;
wait10Msec(55);
servo[Block_Chuck] = 0;
wait10Msec(55);
}
