View unanswered posts | View active topics It is currently Fri Dec 19, 2014 7:27 pm






Reply to topic  [ 18 posts ]  Go to page 1, 2  Next
Gyro PID 
Author Message
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Gyro PID
Hi I am trying to make a PID loop for going straight in robotc.
This is our code so far:
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "gyro.h"
GYRO HDGyro; //Creates Gyro Struct as a global struct
task main()
{
   GyroInit(HDGyro,4); //Change Sensor Port to wherever it is
   int DesiredAngle = 0; //Desired Angle of where you want to be (Set to current Gyro Position)
   float MotorCorrection; //Amount of correction for motors
   int MotorSpeed = 30; //Speed motors start at
   float ProportionalGain = 1.5; //K- value
   while(true){
      GyroTask(HDGyro); //Updates Gyro Variable to current gyro heading
      MotorCorrection = ProportionalGain*(HDGyro.heading - DesiredAngle); //Calculates Motor Correction to make
      nxtDisplayCenteredBigTextLine(4, "%f.2",HDGyro.heading); //Displays the current Gyro Heading
      motor[motorD] = MotorSpeed + MotorCorrection; //Moves and applies motor correction
      motor[motorE] = MotorSpeed + MotorCorrection; //Moves and applies motor correction
      motor[motorF] = MotorSpeed - MotorCorrection; //Moves and applies motor correction
      motor[motorG] = MotorSpeed - MotorCorrection; //Moves and applies motor correction
      wait1Msec(150);
   }
}

So its not working anymore. It worked at first but then now it is slowly curving t when we run in. Is there any way to fix this? I am using MHTS's Gyro driver because it already calculated the heading and integrated the rate against time and there was no noise. I think the problem is the Gyro is not updating because when we watch the screen you can see the Gyro will become something like 1.7 and it will slowly curve and not update back to zero. We also tried to make a tolerance but that didn't work either.


Tue Dec 16, 2014 10:31 am
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Hi just wanted to add one more thing, if any of you know how to use the Sensor MUX with MHTS's gyro.h that would help a lot. Thanks, here is the code to the gyro.h include file we use.
http://proj.titanrobotics.net/hg/Ftc/2013/code/file/fb1661a7bcdf/ftclib/gyro.h We were thinking this could be helpful because in Aswins blog post it said the Gyro varies depending on Voltage and with a sensor Mux it would get a constant voltage from a external battery. Whats weird is we don't have any motors using the NXT battery just a Color Sensor, IR Sensor and the Gyro so we shouldn't be having enough power draw for it to vary the voltage to the Gyro. But it might make sense because it seems like sometimes when running with the motors the Gyro isn't updating as much on the NXT screen and stays stuck at a degree but when we remove the motor code and we move it ourselves it seems quite accurate. Although sometimes it works with the Motors in the beginning. And the first time we tested it it seemed to work.


Tue Dec 16, 2014 5:18 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
To enable the gyro be used with the sensor MUX, you just need to include Xandar's sensor mux driver (hitechnic-sensormux.h). That's it. The only other thing is to make sure when you are calling GyroInit(HDGyro, msensor_S4_1) the sensor ID needs to be of type tMUXSensor such as msensor_S4_1, for example.
In my opinion, I prefer to have the gyro on the NXT directly than through the sensor MUX. This is because if the gyro is on the MUX you need to make sure the MUX must be powered on and the battery voltage is good. There is no way to check the MUX battery programmatically. So if our student forgets to turn the MUX power on or if the battery is running low, our autonomous will fail. In the scenario where I don't have enough sensor ports and must use a sensor MUX, I would put the non-essential sensors on the MUX and the essential ones on the NXT (i.e. in case the MUX has no power and the sensors on the MUX failed, we would only fail partially on our autonomous). For example, we need the gyro to run down the ramp and the touch sensor for the zero calibration of elevator for this year's game. Therefore, they are essential and must be on the NXT. The MUX has two IR seekers and two sonar sensors which are used in the most advanced autonomous strategy. If they malfunction, we still get points just running down the ramp. Touch sensor is required even in teleop to make sure the elevator knows when to stop at the lowest point.
Regarding the gyro stops updating while the motors are running, it sounds like your code wasn't doing multi-tasking correctly. You need to remember that GyroTask is the function that does the integration and must be called periodically. If your robot loop is doing something that caused a wait, the GyroTask doesn't get called in a timely fashion, then the integration stopped and you lose accuracy. In fact, we call the GyroTask in a tighter loop than the normal robot loop that has a wait1Msec in it so it is integrating the gyro in the highest accuracy.
BTW, your code looked fine although the nxtDisplayTextLine code has a wrong format string. You may want to try "%5.1f" instead of "%f.2".
Also, I suspect the robot is not correcting because your K constant is too small. Imagine if your robot is only off by a few degrees, multiplying it with 1.5 will only give you a motor power difference between the left and right wheels of about a few percent. It may or may not be enough to correct the course. Also, our experience found that if there is a mechanical problem with the drive train (e.g. Left wheel has a lot more friction than the right wheel), then the 1.5 K constant can't compensate that. You need to turn all the wheels and feel if one or more of them have more resistance than the others.


Wed Dec 17, 2014 3:21 am
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Ok thanks we will try using multitasking and having 2 separate tasks! Also, do you know if the gyro is accurate enough to be used for absolute heading? That would make it even more accurate in autonomous.


Wed Dec 17, 2014 9:23 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
What do you mean by absolute heading? Gyro is not compass, so it will not be absolute but if you meant the heading relative to the starting heading then yes it is accurate enough since autonomous is only 30 seconds.


Wed Dec 17, 2014 10:47 am
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Ok yes I meant heading relative to the starting point. So I will try to make 2 tasks , 1 with a wait 1Msec(30) calculating the gyro heading in a while loop. And another with a wait1Msec(100) telling the motors what power to run at. What K value did you end up using ,just so we can start testing with a number that has worked before? Have you noticed any accuracy difference with the gyro using a nxt vs a sensor mux with a fully charged battery? Otherwise we will use the NXT ports because we haven't used enough sensors just yet. Thanks, this is really helpful!


Wed Dec 17, 2014 11:22 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
The K value really depends on your robot (the weight, how many motors in the drive train, the friction etc). I would expect it closer to a value of 5. As far as I can tell, there shouldn't be any difference regarding on the gyro accuracy on the NXT port or the sensor mux port.


Wed Dec 17, 2014 2:09 pm
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Ok Thanks for all your help!


Wed Dec 17, 2014 2:39 pm
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Alright so here is my new code. What do you think? I won't be able to test it for a few days so I want to get it all completely ready before I test.
Code:
#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "gyro.h"
typedef struct
{
int DesiredAngle;
float MotorCorrection;
float MotorSpeed;
float ProportionalGain;
}Math;
Math VLF;
GYRO HDGyro; //Creates Gyro Struct as a global struct
task UpdateGyro()
{
   GyroInit(HDGyro,4); //Change Sensor Port to wherever it is
   while(true)
   {
      hogCPU();
      GyroTask(HDGyro);
      releaseCPU();
      wait1Msec(30);
   }
}


task main()
{
   VLF.DesiredAngle = 0; //Desired Angle of where you want to be (Set to current Gyro Position)
   VLF.MotorSpeed = 30; //Speed motors start at
   VLF.ProportionalGain = 5; //K- value
   startTask(UpdateGyro);
   while(true){
      VLF.MotorCorrection = VLF.ProportionalGain*(HDGyro.heading - VLF.DesiredAngle); //Calculates Motor Correction to make
      nxtDisplayCenteredBigTextLine(4, "%f5.1",HDGyro.heading); //Displays the current Gyro Heading
      if(abs(HDGyro.heading-VLF.DesiredAngle) > 0.5){
         motor[motorD] = VLF.MotorSpeed + VLF.MotorCorrection; //Moves and applies motor correction (You may need to adjust this depending on if the sign is correct)
         motor[motorE] = VLF.MotorSpeed + VLF.MotorCorrection; //Moves and applies motor correction
         motor[motorF] = VLF.MotorSpeed - VLF.MotorCorrection; //Moves and applies motor correction
         motor[motorG] = VLF.MotorSpeed - VLF.MotorCorrection; //Moves and applies motor correction
         wait1Msec(150);
      }
   }
}


Wed Dec 17, 2014 8:22 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
Two things:
1. Your format string is still wrong. It should be "%5.1f" not "%f5.1".
2. What is the purpose of this statement?
if(abs(HDGyro.heading-VLF.DesiredAngle) > 0.5){


Wed Dec 17, 2014 9:00 pm
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
1. Ok I will fix that thanks!
2. The purpose is to say that if the error is under .5 then you don't need to correct because the error is so small and it will keep overshooting.
3. Did I do the HogCPU and Release CPU correctly and do I need them? Thanks!


Wed Dec 17, 2014 9:46 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
akash329dsouza wrote:
1. Ok I will fix that thanks!
2. The purpose is to say that if the error is under .5 then you don't need to correct because the error is so small and it will keep overshooting.
3. Did I do the HogCPU and Release CPU correctly and do I need them? Thanks!

Regarding 2, there is really no purpose with that statement. If the error is really small, the correction will be small and you shouldn't have to worry about overshoot. If you have overshoot, K is too large and you need to adjust it down. That's the whole purpose of PID control that if tuned correctly it will maintain your desired angle.
Regarding 3, I wasn't recommending a second task but since you are using another task, it is a good idea to use hogCPU/releaseCPU to ensure exclusive access to the structure by GyroTask. For example, this will make sure the integration will be done atomically. If the task is switching away in the middle of the calculation, it would be a disaster.


Thu Dec 18, 2014 12:14 am
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Ok thank you for your reply but you were saying in the previous post to make sure to call the task periodically and isn't the only way to do that through multiple tasks? Do you know if there is some example cde online I could use to compare to and check with? It would need to be relatively simple as I am a beginner programmer but am the main one on the team. Also thank you so much for awensering all of my questions!


Thu Dec 18, 2014 12:52 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1365
Post Re: Gyro PID
What I meant was something like below. See how the motor code is run every 150 msec but the GyroTask is run a lot more frequent. Note that for this code to work properly, your motor code still need to pay attention not to have any wait or for-loops in it.
Code:
task main()
{
   VLF.DesiredAngle = 0; //Desired Angle of where you want to be (Set to current Gyro Position)
   VLF.MotorSpeed = 30; //Speed motors start at
   VLF.ProportionalGain = 5; //K- value
   unsigned long nextPeriod = nPgmTime;
   while(true){
      GyroTask(HDGyro);
      if (nPgmTime >= nextPeriod) {
         nextPeriod += 150;
         VLF.MotorCorrection = VLF.ProportionalGain*(HDGyro.heading - VLF.DesiredAngle); //Calculates Motor Correction to make
         nxtDisplayCenteredBigTextLine(4, "%5.1f",HDGyro.heading); //Displays the current Gyro Heading
         motor[motorD] = VLF.MotorSpeed + VLF.MotorCorrection; //Moves and applies motor correction (You may need to adjust this depending on if the sign is correct)
         motor[motorE] = VLF.MotorSpeed + VLF.MotorCorrection; //Moves and applies motor correction
         motor[motorF] = VLF.MotorSpeed - VLF.MotorCorrection; //Moves and applies motor correction
         motor[motorG] = VLF.MotorSpeed - VLF.MotorCorrection; //Moves and applies motor correction
      }
      EndTimeSlice();
   }
}


Thu Dec 18, 2014 1:02 am
Profile
Rookie

Joined: Tue Dec 16, 2014 10:25 am
Posts: 20
Post Re: Gyro PID
Ok thank you so much! I will let you know of the result when I am able to test it! (I added something on the next page in case you can't see it!) Thanks!


Last edited by akash329dsouza on Thu Dec 18, 2014 1:23 am, edited 1 time in total.



Thu Dec 18, 2014 1:15 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 18 posts ]  Go to page 1, 2  Next

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.