Wed. Final Week

Today was the day for presentations.
The link for our presentation: WALDO PRESENTATION
And my video of the performance:

After the presentations, we took apart our robots (*sadface*) and inventoried our kits.

Tues. final week

--DANGER-- Possible infinite loop--
I'm writing a blog about Mason presenting a presentation on how to give presentations.

Mason outlined the parameters for how to structure the presentations we have to give on wednesday on our can finding robot.

After that, we spent the rest of the evening working on writing them

Can finding robot code

Here's the final program i used on my can finding robot:

#pragma config(ProgramType, StandaloneWithWiFi)
#pragma config(Sensor, in1,    colorsense,          sensorLineFollower)
#pragma config(Sensor, in2,    line,                sensorLineFollower)
#pragma config(Sensor, in3,    eleft,               sensorRotation)
#pragma config(Sensor, in4,    eright,              sensorRotation)
#pragma config(Sensor, in12,   sonar,               sensorSONAR, int1)
#pragma config(Sensor, in13,   BLU,                 sensorTouch)
#pragma config(Sensor, in14,   GRN,                 sensorTouch)
#pragma config(Sensor, in15,   RED,                 sensorTouch)
#pragma config(Sensor, in16,   ,                    sensorTouch)
#pragma config(Motor,  port1,           right,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           left,          tmotorNormal, openLoop)
#pragma config(Motor,  port3,           clawleft,      tmotorServoStandard, openLoop)
#pragma config(Motor,  port4,           clawright,     tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
//////////////////////////////////////////////////////////////////////////////////////

int nearest=50,inch=11.6,linesense_type=0;
bool override=false,init1=true;
char can_color,ground_color;
char target_can_color,target_ground_color;
bool foundcan=false,can_seek;

///////////////////////////////////////
void clawclose(); void clawopen(); void encoder_clear();
void goforward(int dist, bool length, int power);
void goback(int dist, bool length, int power);
void turnleft(int amount);
void turnright(int amount);
void scan();
char colorcheck();
char foundedge();
//////////////////////////////////////////////
//////////////////////////////////////////////
task find_can()
{
  int tries=0;
  if (init1==true)    // Probably un-needed
  {
    can_seek=true;
    init1=false;
  }
  while(can_seek==true)
  {
     if(foundcan == false)
     {
       if(nearest > 8)       //If the nearest obstacle is more than 8 inches away
       {
         scan(); ////////////
         /////
         //Go forward the distance of the nearest object
         goforward(((nearest)*inch-100),true,50); 
         wait1Msec(1000);
         tries++;
       }
       else if(nearest <=8) //If it detects an object within 8 inches
       {
         goforward((nearest*inch+30),true,50); //Go to the object and grab
         clawclose();
         foundcan = true;    //Set global variable to indicate a can is found
         wait1Msec(1000);
       }
       if(tries>=6)    //If it's tried to find the can 6 times and failed
       {
         turnleft(120);  //Try looking somewhere else
         tries=0;
       }
     }
     else if(foundcan==true)  //If a can is found
     {
        clawopen();  wait1Msec(250); //Get into position to check color
        goback(3,true,30);    //(These numbers need to be tweaked depending on ambient light)
        motor[port8] = 30;    //Point the color sensor at the can
       wait1Msec(500);
       can_color = colorcheck(); //Check the color
       goforward(10,true,50);      //Go grab it again
       clawclose(); wait1Msec(250);
   //What to do with the object now?
       if(can_color == target_can_color) //If it's the right color
       {
        motor[port8]=-35;   //Point color sensor at the ground
        wait1Msec(500);
        linesense_type=1;    //Change the line detection mode
         can_seek=false;      //Stop looking for a can
         tries=0;
         wait1Msec(50);
       }
       else              //Otherwise
       {
        clawopen();     //Release the can
        wait1Msec(1000);
        // FLING THE CAN AWAY >:D //
        motor[left]=-100;
        wait1Msec(250);
         motor[clawright]=-10;
         turnright(20);
         goforward(100,true,70);
         turnleft(20);
         //////////////////
         nearest = 50;
         foundcan = false;     // Start searching again
         tries = 0;
         wait1Msec(50);
       }
     }
  }
  while(can_seek==false)      //When looking for the target zone
  {
   goforward(0,false,80);     //Just go forward
 }
}

////////////////////////////////////////////////
////////// LINE DETECTION "THREAD" /////////////
////////////////////////////////////////////////
task line_detect()
{
  char edge;
  while(linesense_type==0)    //Mode when looking for a can
  {
   if (SensorValue[line] > 750) //The lower bound for dark line
   {
     override = true;
     StopTask(find_can);    // Halt all current activity
     edge = foundedge();   // Run the edge checking function
     if(edge == 'R')       // If it reports the line to be on the right
     {
       turnleft(100);    // Turn left
     }
     if(edge=='L')       // If it reports the line to be on the left
     {
       turnright(100);   // Turn right
     }
     override = false;
     goback(5,true,50);  // Back up a bit
     StartTask(find_can);  //Resume searching
   }
 }
 ///////////////////////////////
 while(linesense_type==1)      //Mode after a can has been found
  { 
   if (SensorValue[line] > 700)
   {
     override = true;
     StopTask(find_can);
     wait1Msec(40);
   goback(1,true,50); //BRAKE
   motor[port8]=-35; // Make sure the color sensor is looking at the ground. (redundant)
   wait1Msec(200);
   ground_color = colorcheck();    //Check the ground color
   if((ground_color==target_ground_color) && (SensorValue[line]<600))
   //If the ground is the target color, and not a floor tile
   {
     clawopen(); //Let it go
     wait1Msec(500);
     goback(100,true,80);
     motor[left]=-120; motor[right]=120; /// Spin for joy!!
     wait1Msec(1000);
     StopAllTasks();     //Shut down
   }
   else if((ground_color != target_ground_color) || (SensorValue[line]>700))
   {
     // If it's not the target color, or it's a floor tile
          goback(325,true,120); wait1Msec(250);   //Go way back
       turnleft(120); wait1Msec(200);          //Turn left and try a different direction
       goforward(0,false,50); wait1Msec(1000);
       StartTask(find_can);
   }
   }
 }
}

task main() // EXECUTED AUTOMATICALLY BY THE VEX BRAIN
{
  goforward(100,true,60);   //Enter the arena
  StartTask(line_detect);   //OTHER TASKS MUST BE INITIALIZED
  StartTask(find_can);
  while(true)           //UNLESS MAIN TASK IN MAINTAINED, EVERYTHING ELSE STOPS
  {
/*SensorValue[RED/GRN/BLU] are simple jumper ports
that read '1' when a jumper is plugged in */
   if(SensorValue[RED]==1)       //Jumper settings for different colors
   {
    target_can_color = 'R';
    target_ground_color = 'R';
   }
   else if(SensorValue[GRN]==1)  
   {
    target_can_color = 'G';
    target_ground_color = 'G';
   }
   else if(SensorValue[BLU]==1)
   {
    target_can_color = 'B';
    target_ground_color = 'B';
   }
   else {StopAllTasks();}
    wait1Msec(1000);
  }
}

//////////////////////////////////////////////////
////////  MAIN ADVANCED FUNCTIONS  ///////////////
//////////////////////////////////////////////////
void scan()
{
  short range = 30;    //Minimum Sonar range. Any less, the bot grabs the can
  short delay = 30;     //mS delay for increments
  short increment = 2;  //amount for each increment
  clawopen();
  wait1Msec(500);
  if(override==false)
  {
    int oldval=50,newval=50;              //intialize the values high for safety

/// Pivot left /////////////////////
   for(int i=0; i<35; i++)
   {
     if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0)) //Sensor sometimes returns -1
     {
       oldval = SensorValue[sonar]-5;      //Set
       goto END;                           //Stop searching
     }
     turnleft(increment);                //Turn left a bit
     newval = SensorValue[sonar];        //Read sonar value into newval
     if((newval<oldval) && (newval > 0)) //if the value is any closer than the previous
     {oldval = newval;}           //Store the value into oldval
     wait1Msec(delay);                   //Pause
   }
   wait1Msec(100);    //Waiting before turning the other direction prevents skidding

   //////////    Repeat the same procedure for the other directions
//// Return to center ////////////////////
   for(int i=0; i<40; i++)
   {
     if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
     {
       oldval = SensorValue[sonar]-5;
       turnright(5);   //Correction to center the sensor
       goto END;
     }
     turnright(increment);
     newval = SensorValue[sonar];
     if((newval<oldval) && (newval > 0)) //if the value is any closer than before
     {oldval = newval;}          //update
     wait1Msec(delay);
   }
/// Continue to pivot right /////////////
   for(int i=0; i<40; i++)
   {
     if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
     {
       oldval = SensorValue[sonar]-5;
       turnright(5);   //Correction
       goto END;
     }
     turnright(increment);
     newval = SensorValue[sonar];
     if((newval<oldval) && (newval > 0)) //if the value is any closer than before
     {oldval = newval;}         //update
     wait1Msec(delay);
   }
   wait1Msec(100);
   
/// Return to center heading ///////////////
   for(int i=0; i<40; i++)
   {
      if ((SensorValue[sonar] < range) && (SensorValue[sonar]>0))
     {
       oldval = SensorValue[sonar]-5;
       goto END;
     }
     turnleft(increment);
     newval = SensorValue[sonar];   //read sonar value into newval
     if((newval<oldval) & (newval > 0)) //if the value is any closer than before
     {oldval = newval;}         //update
     wait1Msec(delay);
   }
  //Set the variable "nearest" to be the smallest value found by sonar
  END:
   nearest = oldval; ///// ***nearest is global***
  }
}

//////////////////////////////////////
///////// Edge avoidance /////////////
//////////////////////////////////////
char foundedge()
{
  long leftval=0,rightval=0;
  int howfar=0;

  goback(25,true,60);             //First action when encountering edge, back up
 turnleft(30);                   //Then turn left
 encoder_clear();                //reset encoders
  for(int i=0; i<35; i++)         /////Check line stuff to the left
  {
    goforward(0,false,40);        //goes forward a bit
    howfar=SensorValue[eleft];    //records the distance
    leftval += SensorValue[line]; //Each iteration, store the line sensor value incrementally
    wait1Msec(10);
  }
  goback(howfar,true,40);         //Back up the amount traveled
  howfar=0;
  turnright(60);                  //Position to check line stuff to the right
  encoder_clear();                //reset encoders
  for(int i=0; i<35; i++)
  {
    goforward(0,false,40);        ///REPEAT
    howfar=SensorValue[eleft];
    rightval += SensorValue[line];
    wait1Msec(10);
  }
  goback(howfar,true,40);         //backup to starting position
  howfar=0;

 if(leftval<rightval)            //Compare the amount of light discovered on each side
 {return 'R';}                   //Return the value of the side that has more dark
 else                            // which will indicate the side the line is on
 {return 'L';}
}

/// Simple color checking function ////
char colorcheck()
{
  char color;
  for(int i=0; i<4; i++)    // Check the value 4 times to eliminate error
  {
   if(SensorValue[colorsense]==11)   // PWM converted signal for RED
   {color = 'R';}
   //PWM converted signal for GREEN oscillates around 340
   if(SensorValue[colorsense]<400 && (SensorValue[colorsense]>300)) 
   {color = 'G';}
   //PWM converted signal for BLUE is around 670
   if(SensorValue[colorsense]<700 && (SensorValue[colorsense]>600))  
   {color = 'B';}
   //PWM converted signal for NOCOLOR is ~1024
   if(SensorValue[colorsense]>950)
   {color = 'W';}
   wait1Msec(250);
  }
  return color;
}
/////////////////////////////////////////////////////
//////// OTHER BASIC FUNCTIONS /////////////////////
/////////////////////////////////////////////////////
void clawclose()
{
  motor[clawright] = -15;  //Tweak these to change the gripping force
  motor[clawleft] = 35;
}
////////////////////
void clawopen()
{
  motor[clawright] = 75;  //NO LARGER THAN 80 FOR EITHER MOTOR,
  motor[clawleft] = -60;  //CHASSIS IN THE WAY
}
/////////////
void encoder_clear() //Zeroes the encoders (**use before moving a specific distance**)
{
  SensorValue[eleft]=0;
  SensorValue[eright]=0;
}
//////////////////////////////////////////////
void goforward(int dist, bool length, int power)
{
//Options to choose between specific distance, or just turn on the motor
//  and how fast to move (power).
  
  if(length == true) //If specific distance req'd
  {
    encoder_clear();
   while((SensorValue[eleft] < dist) && (SensorValue[eright] < dist))
           //while shaft encoders are less than specified distance
   {
     motor[right] = power;   //Engage
     motor[left] = power;
   }
   motor[right] = 0;        //Turn off the motor
   motor[left] = 0;        //Turn off the motor
  }
  
  else if(length == false) //If just turning the motors on
  {
    motor[right] = power;
    motor[left] = power;
  }
}
//////////////////////////////////////////////
void goback(int dist, bool length, int power)
{
  if(length == true)
  {
    encoder_clear();
   while((SensorValue[eleft] < dist)&& (SensorValue[eright] < dist))
   {
     motor[right] = -power;   // ONLY DIFFERENCE FROM ABOVE CODE
     motor[left] = -power;    // IS RUNNING THE MOTORS IN REVERSE
   }
   motor[right] = 0;
   motor[left] = 0;
 }
 else if(length == false)
  {
    motor[right] = -power;
    motor[left] = -power;
  }
}
/////////////////////////////////////
void turnleft(int amount)
{
  //Input value happens to be close to the turn angle in degrees. Nice :D
  encoder_clear();
  while((SensorValue[eright]<amount) && (SensorValue[eleft]<amount))
  {
    motor[right] = 50;  //Right motor forward
    motor[left] = -50;  //Left motor reverse
  }
  motor[right] = 0;     //Stop
  motor[left] = 0;
}
////////////////////////////////////
void turnright(int amount)
{
  encoder_clear();
  while((SensorValue[eright]<amount) && (SensorValue[eleft]<amount))
  {
    motor[right] = -50;   //Right motor reverse
    motor[left] = 50;     //Left motor forward
  }
  motor[right] = 0;       //Stop
  motor[left] = 0;
}
/*/////////////////////////////////////////////    

    --END OF LINE 

*/ 

Week 5

Monday, Tuesday, and Wednesday were spent programming the can finding robot.
My code went through many changes, but I stuck with the same basic plan.
The first objective was to have the robot go out on the field and simply find a can. I did this by making the robot go out and pan left and right recording sonar values, and going forward the smallest amount recorded. It would then repeat this process until it found a value within 7-8 inches, at which point it goes forward and closes the claw.
The first objective was to simply have the bot find the can, grab it, move the can and drop it. This is our attempt:

thursday week 4

First thing in class, we talked about planning and how to stay organized on a project. Me and my teammate got together and made a gant chart for what we needed to accomplish on our robot and when, for it to be ready in time. The first thing we did was to mount all the sensors and get some basic reading code done for them. My color sensor reads a value of 11 for red, 365 for green, 650 for blue, and 1024 for no color.
Our next step was to figure out how we wanted to make a grasping device for the robot. After brainstorming ideas for about 30 -45 min, we started a prototype claw design.
The final claw looks like this:

My partner also added some little finger type things to prevent the can from sliding out.

Wedneday week 4

The color sensor i built was nice, but now we need a way to interface it and do something useful.We modified our circuit to produce a single signal line to interface with the VEX PIC microcontroller. The options were to use the original code and output 2 digital signals (1 or 0) or output an analog signal using a single 3 pin connector. I chose the latter. I modified my code to produce a PWM out signal depending on the color the sensor "sees".
The old output section of the code looked like this:

nocolor:
 serout 0,N2400,("nocolor",13,10)
 high 4,5
goto main

red:
 serout 0,N2400,("red",13,10)
 low 4
 high 5
goto main

blue:
 serout 0,N2400,("blue",13,10)
 high 4
 low 5
goto main

green:
 serout 0,N2400,("green",13,10)
 low 4,5
goto main

I modified it to look like this:

RED:
 pwmout 2, 99, 0  ; Set the Ouput to 0V
goto main

GREEN:
 pwmout 2, 99, 125  ' Set the Output to 1.5 V
goto main

BLUE:
 pwmout 2, 99, 250  ' Set the Outut to 3 V
goto main

NONE:
  pwmout 2,99,400   ' Set the Output to 5V
goto main

The vex brain's sensor ports don't recognize PWM inputs directly, but by adding a capacitor on the PWM signal line, we can change the signal into a (mostly) continuous voltage which the brain can interpret as analog signal. The duty cycle of the PWM signal determines voltage. My next step was to put it all on perfboard so it's nice and compact. After doing this, I'm done! I now have a sensor for our vex robot!

tuesday week 4

Today we worked on building a color sensor for our robot. Using a 14 pin picaxe, I built the circuit on a breadboard and did some test code. The test circuit I built:

When you shine a red, blue, and green light on a red paper, the red light reflects back most brightly, on a blue paper, the blue reflects back most, etc. How this circuit works is that we have a 3 color LED and a CdS cell connected to a pic microcontroller. The 3 colors take turns lighting in sequence and the pic reads the brightness reflected for each one. It then does a comparison of the 3 reflected values and determines what color the object being illuminated is.
The "screenshot" is the serial message i got when holding a green card in front of the sensor, so it works!

I put electrical tape around the CdS cell and LED to reduce error and noise caused by ambient light.
My code:

high 0,1,2,3,4,5
pause 1000
main:
 debug
 w3 = 0
 low 1
 high 2,3
 pause 50
 readadc 0,b0
 low 2
 high 1,3
 pause 50
 readadc 0,b1
 b1=b1+20
 low 3
 high 1,2
 pause 50
 readadc 0,b2
 w3 = b0 + b1 +b2
 if w3 > 450 then nocolor
 if w3 < 220 then nocolor
 if b0<b1 and b0<b2 then red
 if b1<b0 and b1<b2 then green
 if b2<b0 and b2<b1 then blue
goto main
nocolor:
 serout 0,N2400,("nocolor",13,10)
 high 4,5
goto main

red:
 serout 0,N2400,("red",13,10)
 low 4
 high 5
goto main

blue:
 serout 0,N2400,("blue",13,10)
 high 4
 low 5
goto main

green:
 serout 0,N2400,("green",13,10)
 low 4,5
goto main

For this test version, I had a pair of LED's that would light in different pattern to indicate color.