I must reiterate that I had to teach myself "C" before doing this so please don't laugh at my code. I have used lots of floating point arithmetic which I know is processor hungry and not optimally efficient. However, by keeping the algorithms simple, avoiding trigonometry (sines etc) and using a reasonably fast processor, I have a program that cycles quickly enough to do the job. Many builders of this sort of thing are reluctant to put their code on the web as it is obviously possible to injure yourself with a machine like this and such code comes with no guarantees. I do not take responsibility for any accidental injuries you may sustain by building something like this.
//Tilt function lever: If in mid position then stays level, if pushed forwards or back there are some resistors which alter voltage sent to adc3 and adc5 so that the Balance_point value is reset: This makes board balance with nose slightly up or nose slightly down (for going up or down slopes with a relatively low ground clearance ) Balance_point = 520; //i.e. about halfway on the scale of 0-1023, adjust it to suit your machine
/*Subtract offsets*/ x_acc=(float) (accsum/20) - Balance_point; accsum is the sum of 20 sampled accelerometer input values and x_acc is the mean of the 20 values i.e. accsum divided by 20, to give the mean accelerometer reading (on a 0 - 1023 scale)
if (x_acc<-250) x_acc=-250; //cap accel values to a range of -250 to +250 (80 degree tilt each way) if (x_acc>250) x_acc=250;
/* Accelerometer angle change is about 3.45 units per degree tilt in range 0-30 degrees(sin theta) Convert tilt to degrees of tilt from accelerometer sensor. Sin angle roughly = angle for small angles so no need to do trigonometry. x_acc below is now in DEGREES (i.e. converted from a scale of 0 - 1023 where 0=0V input and 1023=5V on an analogue
input pin, to a scale of 0 - 360 degrees which is just easier for us humans to get their heads around)*/
x_accdeg= (float) x_acc/-3.45; //The minus sign corrects for a back to front accelerometer mounting - very easy to do!
/*Subtract offsets: Sensor reading is 0-1023 so "balance point" i.e. my required zero point will be that reading minus 512*/ /*Gyro (Silicon Sensing Systems gyro) angle change of 20mV per deg per sec from datasheet gives change of 4.096 units (on the 0 - 1023 scale) per degree per sec angle change
ganglerate is therefore now converted to DEGREES per second using this value below - it is the RATE of change of angle (from the gyro). This is what the gyro is good at measuring. The accelerometer measures absolute vertical point, a reference point to correct for the fact that the gyro slowly drifts slightly. Gyro great for telling you instantly how fast you are tipping over, but not very good at telling you which way is "up." gangleratedeg=(float)(gyrosum/20 - 508)/4.096;
if (gangleratedeg < -92) gangleratedeg=-92; if (gangleratedeg >92) gangleratedeg=92; I turn port B2 on and off once per main program cycle so I can attach an oscilloscope to it and work out the program cycle time I use the cycle time to work out gyro angle change per cycle where you have to know the length of this time interval PORTB &= (0<<PB2); ti represents a scaling factor for the gyro output (currently with a value of 1.9 here determined by trial and error, although in theory it should simply have a
value of 1)
gyroangledt is anglechange since last CYCLE of the program, in degrees, from gyro sensor, where ti is scaling factor (should be about 1) 0.011 is the time per cycle in seconds as measured using oscilloscope ganglerate is now in units of degrees per second, again, just so we can get our heads around it (we humans are more familiar with the 0-360 degree scale of
angle measurement)
aa varies the amount of the accel reading that is inserted into the equation to gently correct the gyro drift back to the mid (vertical) point, i.e smaller aa value
makes accelerometer time constant longer as it slowly corrects for the gyro drift*/
aa=0.01; gyroangledt = (float)ti*cycle_time*gangleratedeg; gangleraterads=(float)gangleratedeg*0.017453;
new absolute angle of tilt in DEGREES is old tilt angle plus the change in tilt angle (from gyro readings) since last cycle of program
with a little bit of new accel reading factored in to correct for the gyro drift
angle = (float)((1-aa) * (angle+gyroangledt)) + (aa * x_accdeg); the main angle of tilt calculating function (in degrees) Convert angle from degrees to radians (just a scaling issue before we send commands to motor) anglerads=(float)angle*0.017453; The level value is from -1 to +1 and represents the duty cycle to be sent to the motor Incidentally when you are trying to get the hang of all this, the machine will balance and move just fine using this equation alone:
level=(float)((k1*anglerads)*overallgain
balance_torque=(float)(4.5*anglerads) + (0.5*gangleraterads); The anglerads value incorporates the integrated gyro readings (to get actual tilt angle) which have cur_speed = (float)(cur_speed + (Throttle_pedal * balance_torque * cycle_time)) * 0.999; The cur_speed term is nothing to do with balancing. When you are starting out you can just use the term level = balance_torque * overallgain; until you can get your machine to balance, don't worry about this cur_speed term. If you deliberately tip board forwards to make it move, this value allows it to accelerate if you as the rider keep deliberately holding it in a tipped state. The cur_speed value accumulates the longer you hold the board tilted, i.e. it is responsive the TIME board has been tilted in one direction for. By sending more power to the motor in addition the the balance requirements, this build up of extra forward torque (on top of the balance requirements) is necessary to get up slopes for example. The Throttle_pedal term can be altered by a control knob so you can control how briskly it accelerates when you do tilt it and hold it tilted.
The 0.999 multiplier is an ingenious idea from the "SegWii" site. If you are going forward fast for a long "run" and the value for "cur_speed" has accumulated to a large value, when you tilt it back to bring it to a standstill again, you will sometimes find that as you come to a standstill, because the cur_speed variable has not itself come back to exactly zero, the board will be stopped OK but at a fixed angle of backward tilt, i.e. not level as it should be, because you are manually tilting it back to offset this residual cur_speed value. The secret is to make the cur_speed term gently
decay away to zero if it is not being actively reinforced positively with each loop of the program (i.e. you are not actively holding it tipped to make it move forwards). Easy way to do this is to multiply it each time by 0.999 or perhaps 0.9999 (the value the SegWii uses) with each loop of the program.This means when you mave fast then bring it to a stop, it tends to remain level as you slow down to the stationary position. It took me ages to work that bug out!
level = (balance_torque + cur_speed) * overallgain;
So here (above) we take balance_torque (torque to send to motor to get board to balance) and we add to it another value to make it move along. This value is cur_speed and it accumulates gradually, the longer you hold the board tilted for, in your efforts to get it to move along. Without cur_speed term, the board will still move
just fine if you tilt it, but this is a refinement to get it to go up small slopes for example without you having to tilt it to an extreme angle to do so, because it accumulates with time (you hold board tilted a little bit and then WAIT: as cur_speed term accumulates board will eventually move along even if up a hill). When you are starting out just ignore this term altogether from your equations.}
”GOING TOO FAST” BUZZER: Set up LED or buzzer on Port B1 to warn me to slow down if torque to be delivered is more than 70% of max possible The reason for this is that you always need some reserve motor power in case you start tipping forward at speed If motor already running flat-out you would be about to fall over at high speed! Some (Trevor Blackwell) use an auto-tip back routine to automatically limit top speed. For now I will do it this way as easier if (level<-0.7 || level>0.7) { PORTB |= (1<<PB1); //i.e. turn on a warning buzzer } else { PORTB &= (0<<PB1); } softstart = (float) softstart+0.001; if (softstart>1.0) softstart=1.0; Softstart is just a value that increases from a value of 0.4 when board first comes into balanced position, in increments of 0.001 with each loop of the program, until a value of 1 is reached. It stays at 1 thereafter. If you multiply the overallgain by softstart then this means that as you first come to balance position, the board is a little less "jumpy" under your feet until you get used to it, then it gradually "tightens up" during first few seconds of riding it. Trevor Blackwell also had a "too tippy" routine that cuts the power if you are wobbling back and forth too much. These are all refinements and you can do without them when you are initially just trying to build something that balances.
Tilt-start code: Turn on micro while board tipped to one side: Balance algorithm cannot be allowed work yet else board would fly off across the room (as tilted over). You have to have some code that stays dormant when rider is about to step onto it, then, when tilt angle crosses zero (mid) point balance algorithm (i.e. board has just come level as rider tries to tilt it to level point ) it becomes operational.
The software below will just loop forever until board is tipped to level position as rider gets onto board, when level the main balance routine kicks in (but the "kick" is reduced by the initial softstart damping routine above).
Again when you start out, to keep things simple, you can just have a hand controller with a button, push the button when board comes level and you want the balance algorithm to start working, let go to stop it (if you are falling off or it goes haywire).
A note on testing without breaking your ankles: Stand on inactive board in front of a sturdy desk. Lean on desk with arms straight down on desk so desk takes most of your weight and you can lift your feet up quick if you have to. Bring the board level using your feet, while taking most of your weight on your hands onto the desk. When about level, turn on board (thumb controller button on a cable or similar). Get the feel of the thing moving under your feet and how it responds. If it flies off one way or the other, lift your feet up quick! This way you don't break your ankles or head. It might make a dent in the wall either side of you though...............When it get to point where you think it balances, try balancing on it next to a wall, keep your hands on the wall.
NOTE Text below is all true but when it was written accels and gyros ran off a 5V supply from your Arduino. Nowadays they run at 3.3V (also supplied by the more recent Arduino boards), so the voltage the accel puts out when level is no longer 2.5V but more like 1.6V. This means if you are reading it using an analog input pin of your Arduino, "balanced" will no longer be 512 (on the scale of 0 - 1023) but around 352 instead.
The analog input on the microcontroller accepts an input voltage of 0 - 5V from whatever sensor you attach to it. By some sort of convention most sensors give an output of 0-5V The gyro gives a value of 5V (1023 in the micro after converting this voltage to a number on this scale of 0-1023) when rotating (tipping) at maximum rate clockwise, and a value of 0V (0 in the micro on the 0 – 1023 scale) when rotating at maximum rate anticlockwise and 2.5V (about 512 in the micro on the 0-1023 scale) when not rotating at all – i.e. when balanced. Therefore there are 1024 "steps" in our scale of 0-1023 representing an input of 0 - 5Volts. Therefore there are 1024 divided by 5 "steps" on this scale per volt i.e. 204.8 steps per volt. Question: How many "steps" on this scale of 0-1023 represent a rotation of ONE degree per second ? (this is what we actually want to know for the rest of the calculations).Answer: There are 204.8 steps per one volt change. We know my gyro output changes by 20mV (i.e. 0.02 volts) per degree per second, because it says so in the datasheet that come with it.Therefore there will be 204.8 x 0.02 steps on the digital scale of 0 - 1023 per degree per second of gyro rotation (with MY gyro) i.e. = 4.096 steps per degree per second (i.e. for convenience about 4). So for your gyro, If your micro "reads" a 5V input on an analog input pin as a value (to be used in rest of the calculations) of 1023, an input of 0V as a value of 0 and an input of 2.5V as a value of about 512, THEN the calculation is as follows: 204.8 "steps" on this scale per one volt of input from the gyro. If, for example, your gyro changes by 25mV (0.025V) per degree per second rotation speed, then there will be 204.8 x 0.025 steps on the digital scale of 0 - 1023 per degree per second of rotation with YOUR gyro i.e. = 5.12 You could probably just use a value of 5 for simplicity. I suspect the outputs of the gyros are made like this to keep the maths simple - for mine the value is about 4, for others (25mV per degree per second) the value is about 5 "steps" on the 0-1023 scale per degree per second of gyro rotation. Why do we keep subtracting values of 512 (or similar) all through the code?I want a reading (on the 0 - 1023 scale) when balanced, not of about 512 (a 2.5V input from gyro when at rest) but of zero. In the code above I have a term called "Balance Point" with a value of about 512 in the accelerometer calculations (I vary it according to the angle I want this board to be at when balanced i.e. level, nose up or nose down. When you start I would just use a value of 512 and work from there. With the gyro calculations I have used a value of 508 as they gyro did not give 2.5V when stationary but very slightly less than this. |