_gyro=findSensor(sensorI2CHiTechnicGyro) _compass=findSensor(sensorI2CHiTechnicCompass) _compass=findSensor(sensorI2CMindsensorsCompass) MOUNTING can be set to - if the compass or the gyro is mouned upside downįloat meanSensorValue(tSensors s, int interval, int N) Variance for the gyro sensor for different sample sizes * PURPOSE: This program integrates gyro and compass readings using a Kalman filter to get a more * *!!Code automatically generated by 'ROBOTC' configuration wizard !!*// Here is the code, in case you just want to look at it. For this version however I got rid of vector math and switched to normal type of (calculus) functions. This makes the code (and the filter) so much more easy to understand. Initially I used vector math to implement the filter in robotC. But in case you have a multiplexer it should be easy to modify the program. The code probably will not work when the sensors are connected through the sensor multiplexer becuse the function that searches for the sensors only scans the first four sensor ports. The program can find the sensors as long as they are defined with robotC’s motors and sensors setup.Ĭheck the readme and example program called testHeading.c for additional information. Once they are found the filter starts calculating the value of heading. This task will first search for both a compass and a gyro sensor. When you include the code into your program a task will be started automaticly once your program starts. This will create a global variable called heading that contains the filtered heading. To use the filter include heading.c into your program. Now the Kalman filter is working I made the code suitable for use in robotC progams. This post presents the robotC code for the Kalman filter.
0 Comments
Leave a Reply. |
AuthorWrite something about yourself. No need to be fancy, just an overview. ArchivesCategories |