View unanswered posts | View active topics It is currently Sun Apr 20, 2014 11:55 am






Reply to topic  [ 3 posts ] 
Can anyone please help me with this code 
Author Message
Rookie

Joined: Mon Jun 24, 2013 5:15 pm
Posts: 16
Post Can anyone please help me with this code
Hi there,
I was trying to make a program that makes the robot continue to go in the same direction
even if it is piked up and turned to face another direction.
I was going to use the gyro sensor for this so it could track the difference in direction and right its self.
I'm on an FTC team and we wanted to use this encase our robot is steered off track it will continue to go in one direction
I wrote two different programs to execute this but neither work i will post them below.
If you see the problem please reply and you can try it out on your robot if you have a gyro sensor and NXT.
When i run it, it only reads 0 on the screen and doesn't turn when i turn it around

Code:
#pragma config(Sensor, S1,     HTGYRO,              sensorAnalogInactive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

   

#include "../Sample Programs/NXT/3rd Party Sensor Drivers/drivers/hitechnic-gyro.h"

task main ()
{
   float rotSpeed = 0;
   float heading = 0;

   // Calibrate the gyro, make sure you hold the sensor still
   HTGYROstartCal(HTGYRO);   

   // Reset the timer.
   time1[T1] = 0;

   while (true)
   {
      // Wait until 20ms has passed
      while (time1[T1] < 20)
         wait1Msec(1);

      // Reset the timer
      time1[T1]=0;

      // Read the current rotation speed
             rotSpeed = HTGYROreadRot(HTGYRO);   

      // Calculate the new heading by adding the amount of degrees
      // we've turned in the last 20ms
      // If our current rate of rotation is 100 degrees/second,
      // then we will have turned 100 * (20/1000) = 2 degrees since
      // the last time we measured.
      heading += rotSpeed * 0.02;

      //display value of gyro
      nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);
      // move tword the direction first pointing

      while(true)
      {
         while(SensorValue(HTGYRO) == 0 || SensorValue(HTGYRO) == -0)
         {
            motor[motorB] = 80; //forward 80%
            motor[motorC] = 80;
         }
         while(SensorValue(HTGYRO) < 0 || SensorValue(HTGYRO) < -0)
         {
            motor[motorB] = -80; //right 80%
            motor[motorC] = 80;
         }
         while(SensorValue(HTGYRO) > 0 || SensorValue(HTGYRO) > -0)
         {
            motor[motorB] = 80;  //left 80%
            motor[motorC] = -80;
         }
      }

   }
}


Tue Jun 25, 2013 3:52 pm
Profile
Site Admin
Site Admin
User avatar

Joined: Tue Oct 09, 2012 10:34 am
Posts: 187
Post Re: Can anyone please help me with this code
Hi dwarf9668,
Let's separate out a couple of sections of your code:

1.
Code:
      // Wait until 20ms has passed
      while (time1[T1] < 20)
         wait1Msec(1);

      // Reset the timer
      time1[T1]=0;

      // Read the current rotation speed
             rotSpeed = HTGYROreadRot(HTGYRO);   

      // Calculate the new heading by adding the amount of degrees
      // we've turned in the last 20ms
      // If our current rate of rotation is 100 degrees/second,
      // then we will have turned 100 * (20/1000) = 2 degrees since
      // the last time we measured.
      heading += rotSpeed * 0.02;

      //display value of gyro
      nxtDisplayCenteredBigTextLine(3, "%2.0f", heading);


2.
Code:
      while(true)
      {
         while(SensorValue(HTGYRO) == 0 || SensorValue(HTGYRO) == -0)
         {
            motor[motorB] = 80; //forward 80%
            motor[motorC] = 80;
         }
         while(SensorValue(HTGYRO) < 0 || SensorValue(HTGYRO) < -0)
         {
            motor[motorB] = -80; //right 80%
            motor[motorC] = 80;
         }
         while(SensorValue(HTGYRO) > 0 || SensorValue(HTGYRO) > -0)
         {
            motor[motorB] = 80;  //left 80%
            motor[motorC] = -80;
         }
      }



Section 1, when placed inside a loop like you have it, will keep track of the heading of the robot. This part looks fine

Section 2, it should be noted, is running an infinite while loop inside your main loop. This means that once the program reaches this part of the code, it won't ever exit and return to Section 1. Section 2 appears to be a good, if a bit verbose, movement controller. The problem here is that you are controlling the movement using the raw readings from the gyro, which is not returning the heading of the robot (you need the section 1 code for that), but the rate of angular movement, i.e. how fast the robot is turning.

So the first correction is to change your movement controller so that it uses the calculated robot heading instead: change the instances of SensorValue(HTGYRO) in Section 2 to instead reference the heading variable.

The problem now is that Section 2 is still an infinite loop so your program will never return to Section 1 to update the robot heading. Thus, we need to find a way to combine the two sections so they run more-or-less simultaneously (usually said to be executing concurrently, in computer science jargon). The easiest way to do this is to remove your inner loop while(true) loop and then change the while loops that checking the value of SensorValue(HTGYRO) (which we've changed to check the value of heading instead) into if statements.

This will lead to the following program flow:
- Wait for the next multiple of 20ms to have passed
- Read the current rate of rotation from the gyro and use that to update the calculated robot heading
- Use the new calculated heading value to figure out if we should drive left, right or straight for the next 20ms interval
- Repeat

Hopefully that made sense.

Cheers,
--Ryan

_________________
Ryan Cahoon
CMU Robotics Academy
RVW Software Developer

Robot Potato Head; Virtual NXT


Tue Jun 25, 2013 4:09 pm
Profile
Rookie

Joined: Mon Jun 24, 2013 5:15 pm
Posts: 16
Post Re: Can anyone please help me with this code
Thanks so so much for you help. What you said fixed many things but the robot still doesn't work! I was experimenting with some things and it really didnt change much. Here is my code and also read the notes along with it. Also I will make sure to mention your name when i submit my code for next years FTC competition.

keep diggin holes, dwarf9668

Code:
#pragma config(Sensor, S1,     HTGYRO,              sensorAnalogInactive)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

////////////////////////NOTES///////////////////////NOTES////////////////////////
//Spins in circles, not the same direction every time. Think the robot is reading
//the sensor and continues to spin according to the first reading. Most likly
//is the result of the forever loop not repeating for some odd reason.
////////////////////////NOTES///////////////////////NOTES////////////////////////

#include "../Sample Programs/NXT/3rd Party Sensor Drivers/drivers/hitechnic-gyro.h"
//include file for gyro sensor

task main ()
{
   float rotSpeed = 0; //turning speed
   float heading = 0; 

   // Calibrate the gyro, make sure you hold the sensor still
   HTGYROstartCal(HTGYRO);

   // Reset the timer.
   time1[T1] = 0;
   ///RELOOPS every 50 miliseconds///
   while (true)//forever loop//
   {
      // Wait until 20ms has passed
      while (time1[T1] < 50)
         wait1Msec(1);

      // Reset the timer
      time1[T1]=0;

      // Read the current rotation speed
      rotSpeed = HTGYROreadRot(HTGYRO);

      // Calculate the new heading by adding the amount of degrees
      // we've turned in the last 50ms
      // If our current rate of rotation is 100 degrees/second,
      // then we will have turned 100 * (20/1000) = 2 degrees since
      // the last time we measured.
      
      heading += rotSpeed * 0.02;//add heading and rotSpeed and * it by 2%

      //display value of gyro on LCD screen
      nxtDisplayCenteredBigTextLine(3, "Reading of Gyro");
      nxtDisplayCenteredBigTextLine(5, "%2.0f", heading);
 

      //////////////MOVEMENTS///////////////
      // move tword the direction first pointing

      while(heading == 0 || heading == -0)               
      {                                                 
         motor[motorB] = 80; //forward 80% power     
         motor[motorC] = 80;                           
      }                                                 
                                                         
      while(heading < -0) //turn right until gyro = -0
      {
         motor[motorB] = -80; //right 80% power
         motor[motorC] = 80;
      }

      while(heading > 0) //turn left until gyro = 0
      {
         motor[motorB] = 80;  //left 80% power
         motor[motorC] = -80;
      }
   }
}


Tue Jun 25, 2013 4:24 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 3 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.