View unanswered posts | View active topics It is currently Tue Sep 02, 2014 5:22 am






Reply to topic  [ 2 posts ] 
RobotC Code 
Author Message
Rookie

Joined: Fri Sep 04, 2009 3:03 pm
Posts: 10
Post RobotC Code
I could not find a place anywhere else to do this, so I am doing it here. =) I am a senior in high school and currently on VEX team 323A Shockwave and FRC team 1741 Red Alert Robotics. The following is my code for our VEX robot this year....I just wanted to get it out there and see if anyone had any comments on it. Thanks! =D
Code:
#pragma config(Sensor, in1,    lineFollowerRIGHT,   sensorLineFollower)
#pragma config(Sensor, in2,    lineFollowerCENTER,  sensorLineFollower)
#pragma config(Sensor, in3,    lineFollowerLEFT,    sensorLineFollower)
#pragma config(Sensor, dgtl1,  armEncoder,          sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightEncoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  leftEncoder,         sensorQuadEncoder)
#pragma config(Sensor, dgtl7,  clawSwitch,          sensorTouch)
#pragma config(Sensor, dgtl8,  sonarSensor,         sensorSONAR_inch)
#pragma config(Motor,  port1,           leftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port2,           arm1,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           arm2,          tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           ext1Motor,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           ext2Motor,     tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           ,              tmotorNormal, openLoop)
#pragma config(Motor,  port7,           arm3,          tmotorNormal, openLoop)
#pragma config(Motor,  port8,           arm4,          tmotorNormal, openLoop)
#pragma config(Motor,  port9,           clawMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  port10,          rightMotor,    tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

// Declare Functions
void Claw(float c);
void Sonar(float s);
void ArmPos(float ap);
void Forward(float f);
void TurnLeft(float l);
void TurnRight(float r);
void Backward(float b);

// Declare Global Variables     /* 'rotations' will be a counter for every 360 encoder clicks */
const float rotations = 360.0;  /* which is one full rotation of the wheel (ie. 2 'rotations' */
                                /* will equal 720.0 clicks, 2 full rotations of the wheel).   */
int SonarValue;
int ArmLoc = SensorValue(armEncoder);

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                            //
//                          Pre-Autonomous Functions                                          //
//                                                                                            //
// You may want to perform some actions before the competition starts. Do them in the         //
// following function.                                                                        //
//                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
   // All activities that occur before the competition starts
   // Example: clearing encoders, setting servo positions, ...

  SensorValue(rightEncoder) = 0;    /* Clear the encoders for    */
  SensorValue(leftEncoder)  = 0;    /* consistancy and accuracy. */
  SensorValue(armEncoder)   = 0;
}

////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                            //
//                                 Autonomous Task                                            //
//                                                                                            //
// This task is used to control your robot during the autonomous phase of a VEX Competition.  //
// You must modify the code to add your own robot specific commands here.                     //
//                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////

//+++++++++++++++++++++++++++++++++++++++++++++| Autonomous |+++++++++++++++++++++++++++++++++++++++++++++++
task autonomous()
{
  wait1Msec(2000);    // Wait 2000 milliseconds before continuing.

  int i;
  for(i=0; i<1; i++)  // While 'i' is less than 1:
  {
    //Sonar(10.0);     // Call function 'Sonar(float)'     and pass the float value '10.0'  through.
    ArmPos(2.0);     // Call function 'ArmPos(float)'    and pass the float value '1.0'   through.
    Forward(1.0);    // Call function 'Forward(float)'   and pass the float value '1.0'   through.
    Claw(1.0);       // Call function 'Claw(float)'      and pass the bool  value '1.0'   through.
    //ArmPos(40.0);    // Call function 'ArmPos(float)'    and pass the float value '40.0'  through.
    Backward(2.0);   // Call function 'Backward(float)'  and pass the float value '1.0'   through.
    Claw(-1.0);      // Call function 'Claw(float)'      and pass the bool  value '-1.0'  through.
    TurnLeft(1.19);  // Call function 'TurnLeft(float)'  and pass the float value '1.19'  through.
    Forward(2.0);    // Call function 'Forward(float)'   and pass the float value '2.0'   through.
    TurnRight(1.19); // Call function 'TurnRight(float)' and pass the float value '1.19'  through.
  }
 
}

//----------------------------------------| FORWARD FUNCTION |-----------------------------------------
void Forward(float f)
{
  SensorValue(rightEncoder) = 0;    /* Clear the encoders for    */
  SensorValue(leftEncoder)  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while(SensorValue(rightEncoder) < (f * rotations) && SensorValue(leftEncoder) < (f * rotations))
  {
    motor[rightMotor] = 80;   /* Run both motors        */
    motor[leftMotor]  = 80;   /* forward at half speed. */
  }
  motor[rightMotor] = 0;    /* Stop both motors!  This is important so that each function          */
  motor[leftMotor]  = 0;    /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------
//--------------------------------------| CLAW FUNCTION (true) |------------------------------------------
void Claw(float c)
{
  if (c == 1);
  {
  motor[clawMotor] = 127;   /* Run the clawMotor      */
  }                         /* forward at full speed. */

  if (c == -1);
  {
   while (SensorValue(clawSwitch) == 0);
   {
   motor[clawMotor] = -127;  /* Run the clawMotor       */
   }                         /* backward at full speed. */
  }
}
//----------------------------------------------------------------------------------------------------

//----------------------------------------| SONAR FUNCTION |-------------------------------------------
void Sonar(float s)
{
  SonarValue = SensorValue(sonarSensor);

    while(SensorValue(sonarSensor) > s)//Loop while robot's Ultrasonic sensor is further than S inches away from an object
    {
      motor[rightMotor] = 70;         // Motor on port2 is run at 70 power forward
     motor[leftMotor]  = 70;         // Motor on port3 is run at 70 power forward
  }
}
//----------------------------------------------------------------------------------------------------
//----------------------------------------| ARMPOSITION FUNCTION |-----------------------------------------
void ArmPos(float ap)
{
  ArmLoc = SensorValue(armEncoder);
  // While the encoder has not yet met its goal
  while(ArmLoc < ap)
  {
    motor[arm1] = 70;
    motor[arm2] = 70;
    motor[arm3] = 70;
    motor[arm4] = 70;
    ArmLoc = SensorValue(armEncoder);
  }
  // While the encoder has not yet met its goal
  while(ArmLoc > ap)
  {
   motor[arm1] = -70;
   motor[arm2] = -70;
   motor[arm3] = -70;
   motor[arm4] = -70;
   ArmLoc = SensorValue(armEncoder);
  }
    motor[arm1] = 0;   /* Stop the motor!  This is important so that each function          */
    motor[arm2] = 0;   /* can act independantly as a "chunk" of code, without any loose ends. */
    motor[arm3] = 0;
    motor[arm4] = 0;
}
//----------------------------------------------------------------------------------------------------


//----------------------------------------| BACKWARD FUNCTION |----------------------------------------
void Backward(float b)
{
  SensorValue(rightEncoder) = 0;    /* Clear the encoders for    */
  SensorValue(leftEncoder)  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (r * rotations) ie (3.0 * 360.0) or "three rotations"
  while(SensorValue(rightEncoder) > -(b * rotations) && SensorValue(leftEncoder) > -(b * rotations))
  {
    motor[rightMotor] = -80;   /* Run both motors        */
    motor[leftMotor]  = -80;   /* forward at half speed. */
  }
  motor[rightMotor] = 0;    /* Stop both motors!  This is important so that each function          */
  motor[leftMotor]  = 0;    /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------

//---------------------------------------| TURN LEFT FUNCTION |----------------------------------------
void TurnLeft(float l)
{
  SensorValue(rightEncoder) = 0;    /* Clear the encoders for    */
  SensorValue(leftEncoder)  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
  while(SensorValue(rightEncoder) < (l * rotations) && SensorValue(leftEncoder) > (-1 * l * rotations))
  {
    motor[rightMotor] = 80;   // Run the right motor forward at 100 speed
    motor[leftMotor]  = -80;  // Run the left motor backward at 100 speed
  }
  motor[rightMotor] = 0;    /* Stop both motors!  This is important so that each function          */
  motor[leftMotor]  = 0;    /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------

//---------------------------------------| TURN RIGHT FUNCTION |---------------------------------------
void TurnRight(float r)
{
  SensorValue(rightEncoder) = 0;    /* Clear the encoders for    */
  SensorValue(leftEncoder)  = 0;    /* consistancy and accuracy. */

  // While the encoders have not yet met their goal: (left is compared negativly since it will in reverse)
  while(SensorValue(leftEncoder) < (r * rotations) && SensorValue(rightEncoder) > (-1 * r * rotations))
  {
    motor[rightMotor] = -80;   // Run the right motor forward at half speed
    motor[leftMotor]  = 80;  // Run the left motor backward at half speed
  }
  motor[rightMotor] = 0;    /* Stop both motors!  This is important so that each function          */
  motor[leftMotor]  = 0;    /* can act independantly as a "chunk" of code, without any loose ends. */
}
//----------------------------------------------------------------------------------------------------


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                            //
//                                 User Control Task                                          //
//                                                                                            //
// This task is used to control your robot during the user control phase of a VEX Competition.//
// You must modify the code to add your own robot specific commands here.                     //
//                                                                                            //
////////////////////////////////////////////////////////////////////////////////////////////////


//=============================================| DRIVE |==============================================\\
task Drive()
{
  int joy_l;            // Will hold the X value of the LEFT analog stick.
  int joy_r;            // Will hold the X value of the RIGHT analog stick.
  int threshold = 20;   // Helps to eliminate 'noise' from a joystick that isn't perfectly at (0,0)

  while(true)
  {

    joy_l = vexRT[Ch3];   //This gets the X axis value of the LEFT analog stick and assigns the value to "joy_l".
    joy_r = vexRT[Ch2];   //This gets the X axis value of the RIGHT analog stick and assigns the value to "joy_r".

////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If joy_l is above the threshhold then assign that vale to leftMotor, if not leftMotor power equals 0.   //
                                                                                                          //
    if(abs(joy_l) > threshold)                                                                            //
    {                                                                                                     //
      motor[leftMotor]  = (joy_l);                                                                        //
    }                                                                                                     //
                                                                                                          //
    else                                                                                                  //
    {                                                                                                     //
      motor[leftMotor]  = 0;                                                                              //
    }                                                                                                     //
                                                                                                          //
////////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////////////////////////
//If joy_r is above the threshhold then assign that vale to rightMotor, if not rightMotor power equals 0. //
                                                                                                          //
    if(abs(joy_r) > threshold)                                                                            //
    {                                                                                                     //
      motor[rightMotor]  = (joy_r);                                                                       //
    }                                                                                                     //
                                                                                                          //
    else                                                                                                  //
    {                                                                                                     //
      motor[rightMotor]  = 0;                                                                             //
    }                                                                                                     //
                                                                                                          //
////////////////////////////////////////////////////////////////////////////////////////////////////////////

    wait1Msec(25);      // helps the multitasking share evenly

  }
}
//====================================================================================================\\


//==============================================| Arm |===============================================\\
task Arm()
{
 {
  int armPosition = SensorValue(armEncoder);

  while(true)
   {
/////////////////////////////Claw Movement code////////////////////////////////////////////////

    /* if (SensorValue(clawSwitch) == 1);
     {
      motor(clawMotor) = 127;
     }
     
     if (SensorValue(clawSwitch) == 0);
     {
      motor(clawMotor) = 0;
     }*/
     
     
     
     
    if (vexRT[Btn8LXmtr2] ==  1)
    {
     motor(clawMotor) = 127;
    }
    else
    {
      if (vexRT[Btn8RXmtr2] ==  1)
      {
        //while(SensorValue(clawSwitch) == 1);
        //{
         motor(clawMotor) = -100;     
        //}
      }
    }
   
    if ((vexRT[Btn8LXmtr2] == 0) && (vexRT[Btn8RXmtr2] == 0));
    {
     motor(clawMotor) = 0;
    }

///////////////////////////End Claw Movement code////////////////////////////////////////////////

//////////////////////////////Arm Movement code//////////////////////////////////////////////////

    if (vexRT[Btn8UXmtr2] ==  1)
    {
     motor[arm1]  = 100;
     motor[arm2]  = 100;
     motor[arm3]  = 100;
     motor[arm4]  = 100;
    }

    if (vexRT[Btn8DXmtr2] ==  1)
     {
      motor[arm1]  = -100;
      motor[arm2]  = -100;
      motor[arm3]  = -100;
      motor[arm4]  = -100;
     }

     if ((vexRT[Btn8UXmtr2] ==  0) && (vexRT[Btn8DXmtr2] ==  0))
     {
      motor[arm1]  = 0;
      motor[arm2]  = 0;
      motor[arm3]  = 0;
      motor[arm4]  = 0;
     }

///////////////////////////////End Arm Movement code////////////////////////////////////////////////

/////////////////////////////Hang Arm Extention code////////////////////////////////////////////////


    if (vexRT[Btn5DXmtr2] ==  1)
    {
     motor(ext2Motor) = 127;
    }
    else
    {
     motor(ext2Motor) = 0;
    }

    if (vexRT[Btn5UXmtr2] ==  1)
    {
     motor(ext2Motor) = -127;
    }


    if (vexRT[Btn6DXmtr2] ==  1)
    {
     motor(ext1Motor) = 127;
    }
    else
    {
     motor(ext1Motor) = 0;
    }

    if (vexRT[Btn6UXmtr2] ==  1)
    {
     motor(ext1Motor) = -127;
    }

///////////////////////////End Hang Arm Extention code////////////////////////////////////////////////

  }
 }
}
//====================================================================================================\\

task usercontrol()
{

//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++\\

  StartTask(Drive);

  StartTask(Arm);

  int batteryLevel;  //Variable to hold the battery level.

  while(true)         // infinite loop, to keep the program alive
  {

    batteryLevel = (nAvgBatteryLevel);  //"batteryLevel" equals average battery level in Mili-Volts

    wait1Msec(25);      // helps the multitasking share evenly
  }

 //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\\
}

_________________
Jordan Miller
2010 Dean's List Semi-Finalist
FRC 1741 --- Engineering Captain, Driver
Vex 323a --- Team Captain, Driver


Thu Nov 18, 2010 10:48 pm
Profile
Expert

Joined: Mon Oct 27, 2008 9:59 pm
Posts: 137
Post Re: RobotC Code
Dracco,
First off, great code. Extremely well laid out, formated, and documented. Better than a lot of professional code I review.

I do not have any experience with ROBOTC on Vex, but I assume it is about the same as on the NXT. I do wonder if your post would get more or less attention if it was in the Vex sub-forum?

That said, some random thoughts/comments while skimming your code...

1) I see that you're a big fan of floats in your function params. In at least one case, I think you may want to consider a more appropirate datatype such as passing an enum to your claw() function. Passing something like CLAW_OPEN or CLAW_CLOSE is more readable than 1 and -1.

2) You have a lot of hard coded values throughout the code (generally motor speeds, encoder targets, and the like). I would suggest using constants for these values as it is easier to read and if the value is used more than one place and you have to update it, you only have to remember to update the one constant. ROBOTC provides the keyword 'const' for defining constants so that you do not have to use the old #define.

3) There are places where you are setting a single output (or set of outputs such as two or four motors) with multiple independent conditional statements in a single function. Generally, you only want one independent set of conditions to control that type of output. For example, in your claw movement code, you have two independent sets of conditions all controlling a single clawMotor output. It would be cleaner to combine them in a single if/else if/else construct to avoid ambigous situations where the last conditional wins.
For example, this:
Code:
     
    if (vexRT[Btn8LXmtr2] ==  1)
    {
     motor(clawMotor) = 127;
    }
    else
    {
      if (vexRT[Btn8RXmtr2] ==  1)
      {
        //while(SensorValue(clawSwitch) == 1);
        //{
         motor(clawMotor) = -100;     
        //}
      }
    }
   
    if ((vexRT[Btn8LXmtr2] == 0) && (vexRT[Btn8RXmtr2] == 0));
    {
     motor(clawMotor) = 0;
    }

Could be re-written as this:
Code:
   if(vexRT[Btn8LXmtr2])
   {
      motor(clawMotor) = 127;
   }
   else if(vexRT[Btn8RXmtr2])
   {
      motor(clawMotor) = -100;     
   }
   else
   {
      motor(clawMotor) = 0;
   }

I would also ask what happens in your code if both vexRT[Btn8LXmtr2] and vexRT[Btn8RXmtr2] were to return 1. Just something to think about...

So those are just a few thoughts to make your already good code maybe a little bit better. Great job.


Thu Dec 02, 2010 6:35 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 2 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.