View unanswered posts | View active topics It is currently Sat Oct 25, 2014 3:47 pm






Reply to topic  [ 6 posts ] 
Need Assistance With First Robot C Program 
Author Message
Rookie

Joined: Sat Jan 04, 2014 2:08 am
Posts: 3
Post Need Assistance With First Robot C Program
I am working to build my first real Mindstorms robot and am running into a few issues.

Currently, I have:
  • a tracked robot (one left motor, one right motor)
  • a motor that turns a rotating mast with an ultrasonic sensor on top
  • a static ultrasonic sensor on the front.

I would like the robot to continuously move forward until either ultrasonic sensor detects an obstacle in front of it.

Then I would like the robot to stop and rotate the mast up to 180 degrees to the left or right to find the optimal clear path.

Once a clear path is found, I would like to rotate the vehicle to point to that path.

I would then like to return to moving forward until the next obstacle is found (essentially loop back to its initial operating state).

I have been periodically tinkering with the code in my free time, but my find clear path logic needs some help. Right now, once an obstacle is detected, the mast goes spastic turning left and right but not turning the vehicle to the clear path.

Here is my code right now. I would appreciate input from some experienced Mindstorms programmers. I do have some of the movements commented out and replaced with outputs to the display. It was easier than chasing the robot around the family room while I was trying to figure out the issues.

Code:
#pragma config(Sensor, S1, sensorSonarMast, sensorSONAR)
#pragma config(Sensor, S2, sensorSonarStatic, sensorSONAR)
#pragma config(Sensor, S3, sensorGPS, sensorCustom)
#pragma config(Sensor, S4, sensorWiFi, sensorCustom)
#pragma config(Motor, motorA, motorLeft, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, motorB, motorRight, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, motorC, motorMast, tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//int sensorSonarStaticValue = 0;
int sensorSonarMastValue = 0;


void ResetMastEncoder()
{
         nMotorEncoder[motorC] = 0;
}

void MoveMast(int targetAngle)
{
   while(nMotorEncoder[motorC] != targetAngle)
   {
      motor[motorC] = (targetAngle - nMotorEncoder[motorC]);
   }
   motor[motorC] = 0;
}

int CheckLeft(int minimumDistance = 5, int angleOfScan = 5){
   MoveMast(angleOfScan * -1);
   sensorSonarMastValue = SensorValue(sensorSonarMast);
   MoveMast(0);
   if( sensorSonarMastValue > minimumDistance )
      return (angleOfScan * -1);
   else
      return 0;
}

int CheckRight(int minimumDistance = 5, int angleOfScan = 5){
   MoveMast(angleOfScan);
   sensorSonarMastValue = SensorValue(sensorSonarMast);
   MoveMast(0);
   if( sensorSonarMastValue > minimumDistance )
      return angleOfScan;
   else
      return 0;
}


void RotateVehicle(int degreesToRotate = 180)
{
   nMotorEncoder[motorLeft] = 0;
   nMotorEncoder[motorRight] = 0;
   float leftRotationLimit = degreesToRotate * 0.01;
   //float rightRotationLimit = leftRotationLimit * -1;
   while(nMotorEncoder[motorLeft] < leftRotationLimit)
   {
         motor[motorLeft] = 25;
         motor[motorRight] = -25;
         nxtDisplayTextLine(4,"Motor: %d", nMotorEncoder[motorLeft]);
   }
}

void MoveForward(int minimumDistance = 30)
{
   int sensorSonarStaticValue = 0;
   int sensorSonarMastValue = 0;
   

   while(true)
   {
      int angleOfScan = 0;
      int clearPathFoundAt = 0;
      sensorSonarStaticValue = SensorValue(sensorSonarStatic);
      sensorSonarMastValue = SensorValue(sensorSonarMast);

      if ((sensorSonarStaticValue > minimumDistance || sensorSonarStaticValue == -1) || (sensorSonarMastValue > minimumDistance || sensorSonarMastValue == -1))
      {
            // NO OBJECTS WITHIN MINIMUM DISTANCE
            motor[motorLeft] = 100;
           motor[motorRight] = 100;
      }
      else
      {
            // OBJECT DETECTED WITHIN MINIMUM DISTANCE
            // SCAN LEFT AND RIGHT IN 5 DEGREE INCREMENTS TO FIND A NEW ROUTE
            while(clearPathFoundAt == 0 && angleOfScan < 180)
           {
                 angleOfScan += angleOfScan + 5;
               clearPathFoundAt = CheckLeft(minimumDistance, angleOfScan);
               clearPathFoundAt = CheckRight(minimumDistance, angleOfScan);
             }
      }
      nxtDisplayTextLine(1,"Static: %d",sensorSonarStaticValue);
      nxtDisplayTextLine(2,"Mast: %d",sensorSonarMastValue);
      wait1Msec(1000);
      RotateVehicle(clearPathFoundAt);
   }
}

task main()
{
   bMotorReflected[motorA] = true; // FLIP NORMAL MOTOR DIRECTIONS
   bMotorReflected[motorB] = true; // FLIP NORMAL MOTOR DIRECTIONS
   bFloatDuringInactiveMotorPWM = false; // BRAKE MOTOR TO ALLOW MORE PRECISE CONTROL
   MoveForward(12);
}


Last edited by donein on Sun Jan 05, 2014 2:36 pm, edited 4 times in total.



Sat Jan 04, 2014 2:27 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3227
Location: Rotterdam, The Netherlands
Post Re: Need Assistance With First Robot C Program
It would be better if you pasted code that could compile:
Code:
*Warning*:Undefined procedure 'checkRight'. Global subroutine assumed.
**Error**:Undefined procedure 'checkRight'
*Warning*:Undefined procedure 'checkLeft'. Global subroutine assumed.
**Error**:Undefined procedure 'checkLeft'

Please repost your code with the missing functions as well, please.

= Xander

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Sat Jan 04, 2014 3:15 am
Profile WWW
Rookie

Joined: Sat Jan 04, 2014 2:08 am
Posts: 3
Post Re: Need Assistance With First Robot C Program
mightor wrote:
It would be better if you pasted code that could compile:
Code:
*Warning*:Undefined procedure 'checkRight'. Global subroutine assumed.
**Error**:Undefined procedure 'checkRight'
*Warning*:Undefined procedure 'checkLeft'. Global subroutine assumed.
**Error**:Undefined procedure 'checkLeft'

Please repost your code with the missing functions as well, please.

= Xander


Whoops, bad cut and paste. I had posted completely the wrong revision. I updated the code in my post above.


Sat Jan 04, 2014 7:24 pm
Profile
Expert
User avatar

Joined: Sat Aug 31, 2013 9:15 am
Posts: 138
Post Re: Need Assistance With First Robot C Program
Well, I can see a problem in your code right off the bat:
Code:
while(false && angleOfScan < 180)

           {
                 angleOfScan += angleOfScan + 5;
               CheckLeft(minimumDistance, angleOfScan);
               CheckRight(minimumDistance, angleOfScan);
           }


Those lines of code inside the while loop will never execute because of the while(false && angleOfScan < 180). A while executes while the condition in the parenthesis is true. That condition will never be true. Simply remove the "false" part of it, and the loop will execute.

_________________
FTC Team 6100 Chariots of Fire - Programmer (2012-2013)
FTC Team 7468 Blue Chariots of Fire - Programmer (2013-2014)
FTC Team 7468 Blue Chariots of Fire - Mentor (2014-2015)
Check out our team website at http://cof7468.weebly.com/.


Sun Jan 05, 2014 10:15 am
Profile
Rookie

Joined: Sat Jan 04, 2014 2:08 am
Posts: 3
Post Re: Need Assistance With First Robot C Program
I made some changes (initial post changed), but it still doesn't seem to work. The robot just seems to just be trucking along headlong into the obstacle.


Sun Jan 05, 2014 2:28 pm
Profile
Expert
User avatar

Joined: Sat Aug 31, 2013 9:15 am
Posts: 138
Post Re: Need Assistance With First Robot C Program
I can see why your robot doesn't stop. You don't tell it to! At the beginning of the else statement that executes when an object is detected, set your drive motors' powers to zero.

Another thing I see in your scanning loop is that the values returned by CheckLeft and CheckRight are stored in the same variable. So, if your robot finds a clear path to the left, that will promptly be overwritten by a non-clear path to the right. You might want to consider storing the left path and right path in separate variables.

_________________
FTC Team 6100 Chariots of Fire - Programmer (2012-2013)
FTC Team 7468 Blue Chariots of Fire - Programmer (2013-2014)
FTC Team 7468 Blue Chariots of Fire - Mentor (2014-2015)
Check out our team website at http://cof7468.weebly.com/.


Mon Jan 06, 2014 11:00 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 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.