But despite my lack of software, I have still been able to make strides in my NXC implementation. There's too much code to paste here, so you'll need to download it if you'd like to follow along. This control is separated into four parallel tasks called getAccelData(), getGyroData(), getButtons(), and setMotorSpeed(). Let's take a look at each one in turn.
setMotorSpeed()
First there's setMotorSpeed(). This code is based off of the control code from a couple weeks ago. In its current state, the code acts as a P controller, and doesn't rely on derivative or integral term. The global values for the ideal angle and the current angle are read, the error is calculated, and run through the control law. The proportional gain is set to 20 by experimentation. The output is then used to set the motor speed as before.
I attempted to use the Ziegler-Nichols method again for tuning, but the gains it provided never lead to the robot coming close to balancing. Instead it would violently oscillate around the equilibrium point. Also, enabling the integral gain with any reasonable value caused the integral term to run away, which I believe to be mostly due to the gyro drift I'll discuss a little later. I decided to disable the Ziegler-Nichols tunings, and the integral term, to try to find a proportional gain that came closed to balancing.
getGyroData()
Before running the getGyroData() task, the initGyro() function is called. This performs the same calibration that I wrote last week.
When getGyroData() is run, the sensors raw value is read. It is filtered into the offset using a weighted sum in a similar manor to the HTWay I analyzed earlier. This regulated value is used as the offset to determine the gyro's actual rotation speed. The speed is integrated into the angle using the approximate time since the last call in the loop. The gyro's offset, speed, and angle are all globals set by this task. Other tasks have read access to them, most importantly, setMotorSpeed().
I had a hard time adjusting the weighting for the filter on the gyro's offset. The value in the HTWay example was set for a configuration different from mine. Since there was little explanation as to how they came to their value, I experimented with values that seemed appropriate. This version of the code has 0.01 which is two orders of magnitude larger that the example's code. I believe this to be necessary due to the different sample rates between the two. Mine is running considerably faster, and may need to be adjusted further.
getAccelData()
This task is actually disabled when compiled as indicated by the first line in the task itself. This was an experiment in resetting the ideal angle for balancing by using the accelerometer, but was unreliable. I believe the idea to be in the right direction, but it's difficult to say.
After initializing the sensor, the tasks main loop begins. The loop reads the accelerometer and calculates the approximate value in Gs experienced in the x (up/down) and the y (forward/backward) directions. It then calculates the derivative of the acceleration, otherwise known as "jerk". Using these values, the ideal angle is reset to the gyro's current sensed angle only if the acceleration in the forward / backward direction is near the value it should be for balancing, and the jerk is near zero indicating the acceleration isn't changing.
I found this to work alright, but it wasn't consistent. Using this method, the upright position can only be detected when the robot is already nearly balanced. If the platform is oscillating around balanced, the ideal angle will never be improved. I haven't tried combining these sensors before, so I figured it might be worth it to take a shot. But since this isn't the focus of this project. I've left this disabled and will move on to something else.
getButtons()
A very simple task, as a debugging alternative, when the left button is held, it constantly resets the ideal angle with the gyro's current angle. This isn't used as part of the control, but was used when testing other components.
Performance
As it operates now, the robot nearly balances, but is effected by the drift of the gyro too much to maintain it's balance. If I run the program and hold the robot in the air nearly balanced, the wheels start stopped but slowly accelerate to full speed in some direction. This gyro drift must be addressed before I can implement a control, with either BricxCC or Matlab.
Looking into the matter more deeply, I came across this forum post on the Mindboards forum, a general Mindstorms discussion board. That particular thread has gone on for nearly a year and has a lot of interesting discussions on the performance and considerations of gyroscopic sensors in general. Most notably for me, it seems that people are able to use the weighted sum without too much trouble. It also obviously works for HiTechnic's example build. There's probably a more appropriate value for that weighting that I haven't found yet.
I'm feeling a little burnt out on this gyro issue for now. Since I have Matlab, I think I'll shift back to exploring the evolutionary algorithms application to this robot.
Shifting to Matlab / Simulink
From my previous work, I already have a model ready to test of a full state feedback controller in the loop with a plant. I could run the evolutionary algorithm over this, but it wont provide much more utility over the existing built-ins with the software (e.g. it's trivial to call Matlab's place or acker to find poles). Instead, I'll try to give the algorithm a more challenging problem by adding some motor dynamics and non-linearities to the the model before proceeding.
No comments:
Post a Comment