2/9/2011

Today i was working on the range sensor that took forever to understand. i was very frustrated trying to get the code to work. and just getting the robot to move.

#pragma config(Sensor, in1,    LineDetector,        sensorLineFollower)
#pragma config(Sensor, in2,    colorSensor,         sensorReflection)
#pragma config(Sensor, in5,    SonarSensor,         sensorSONAR, int1)
#pragma config(Sensor, in7,    LeftEncoder,         sensorRotation)
#pragma config(Sensor, in8,    RightEncoder,        sensorRotation)
#pragma config(Motor,  port1,           LeftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port2,           RightMotor,    tmotorNormal, openLoop)
#pragma config(Motor,  port3,           hand,          tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void goForward(int distance)
{
  motor[port1] = 40;
  motor[port2] = 30;
  wait10Msec(distance);
}

void goLeft(int distance)
{
  motor[port1] = 60;
  motor[port2] = -40;
  wait10Msec(distance);
}

void goRight(int distance)
{
  motor[port1] = -60;
  motor[port2] =40;
  wait10Msec(distance);
}


void goStop(int distance)
{
    motor[port1] = 0;
    motor[port2] = 0;
    wait10Msec(distance);
}

void goback (int distance)
{
  motor [port1]=-50;
  motor [port2]=-53;
  wait10Msec(distance);

}

void grab()
{
  motor [hand]= -60;
  wait10Msec(50);

}

void letGo()
{
  motor [hand]= 0;
  wait10Msec(50);
}

void grabCan()
{
     goStop(100);
     goForward(100);
     grab();
     goback(100);
     goStop(100);
     letGo();
     goback(100);
}


task main
{
    wait1Msec(2000);                            //Robot waits for 2000 milliseconds before executing program
      bMotorReflected[port2] = 1;        //Reflects the direction of the motor on port2

      SensorValue[RightEncoder] = 0;    //Set the encoder so that it starts counting at 0
      SensorValue[LeftEncoder]  = 0;
      int test = SensorValue(colorSensor);

      bool go = true;
      int oddCheck = 1; //used for remainders to check if odd or left
      bool checkOdd = true;
      int remainder;    //if remainder is odd it goes left if even then right
      int turnCheck = 0;   //Used to check if the robot has gone around in a circle to much
                        //if the turnCheck is more than 10 it will go straight
      goForward(40);
while(go)
 {

   int range = SensorValue(SonarSensor);
   if (range > 8 && range < 35)
   {
     goForward(30);
     if (checkOdd == true)
     {
        oddCheck++;
        remainder = oddCheck%2;
        checkOdd = false;
     }
   }
   if ( range < 0 || range > 35)
   {
     if (remainder==0)
    {
     checkOdd = true;
     goLeft(1);
     turnCheck++;
     if(turnCheck >500)
     {
       goStop(20);
       goForward(20);
       turnCheck = 0;
     }
    }
    if (remainder!=0)
    {
      checkOdd = true;
      goRight(1);
      turnCheck++;
      if(turnCheck>500)
      {
        goForward(20);
        goStop(20);
        turnCheck = 0;
      }
    }
   }
   if (range < 8 && range>0)
   {
      grabCan();
      go = false;
   }
 }
}


By the end of the day i relized that this code is not as great. it still can not find the can's. Also from talking to a lot of the groups i have my max range of good vales (range < 0 || range > 35) set much higher than a lot of groups. but i find it that it helps to search for at least something. but i can see that it detects the walls to much or peoples feet.