|
CODE 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.
January 09 Features: 1) Push button kill switch 2) Control knob is now the "Throttle pedal" controls speed of acceleration to top speed 3) 2 control knobs for linear throttle and overall gain 4) ti slightly increased over previous versions for tighter feel, less tilt to make it move along 5) Softstart working. 6) Comes on when perfectly level now. 7) Designed for the low ground clearance mechanical design 8) First attempt with new controller with tilt lever.TILT WORKING
Wires to hand controller: +5V Red A2 grey wire OverallGain control via CUT OUT SWITCH A3 orange wire Tilt lever back A4 green wire Throttle control knob A5 blue wire Tilt lever forwards
Micro is ATMega32 #include <avr/io.h> #include <avr/pgmspace.h> #include <avr/interrupt.h> #include <math.h> #define CLOCK_SPEED 16000000 #define OCR1_MAX 1023 typedef unsigned char u8; void set_motor_idle(void); void InitPorts(void); float level=0; float Throttle_pedal; float aa; float x_acc; float accsum; float x_accdeg; float gyrosum; float gangleratedeg; float gangleraterads; float ti = 2.2; float overallgain; float gaincontrol; float batteryvolts = 24; float gyroangledt; float angle; float anglerads; float balance_torque; float softstart; float cur_speed; float cycle_time = 0.0104; float Balance_point; int q; int i; int tipstart; void InitPorts(void) { PORTC=0x00; //Port C pullups set to low (no output voltage) to begin with DDRC=0xFF; //Port C pins all set as output via the port C direction register
DDRA=0x00; //all port A pins set as input PORTA=0x00; //Port A input pullups set to low pullups DDRD=0xFF; //Configure all port D pins as output as prerequisite for OCR1A (PinD5) and OCR1B (Pin D4) working properly PORTB=0x00; //Port B pullups set to low (no output voltage) to begin with DDRB=0xFF; //All port B pins set to output } IO: I am using ATMega32 16MHz with external crystal clock. New planned pin arrangement to OSMC motor controller
PD5/OC1A ALI -> OSMC pin 6
PD4/OC1B BLI -> OSMC pin 8
PC1 Disable -> OSMC pin 4 PC2 BHI -> OSMC pin 7 PC3 AHI -> OSMC pin 5
PB2 Pulse to socket for connection to oscilloscope, one pulse per cycle of program
Inputs: PA0/ADC0 accelerometer PA1/ADC1 pitch rate gyro PA2/ADC2 overallgain (via cutout switch) PA2
PA3/ADC3 Position lever pulled back position PA3
PA4/ADC4 "Throttle Pedal" control knob - controls how fast it accelerates when deliberately tipped by rider PA4
PA5/ADC5 Position lever pushed forwards position PA5
void adc_init(void) { /* turn off analogue comparator as we don't use it */ ACSR = (1 << ACD);
/* select PA0 */ ADMUX = 0; ADMUX |=(1<<REFS0); //This tells it to use VCC (approx 5V) as the reference voltage NOT the default which is the internal 2.5V reference /* Set ADC prescaler to 128, enable ADC, and start conversion */ ADCSRA = 0 | (1<<ADPS2) | (1<<ADPS1) | (1<<ADPS0) | (1<<ADEN) //enable ADC | (1<<ADSC); //start first conversion /* wait until bogus first conversion finished */ while (ADCSRA & (1 << ADSC)) { } } uint16_t adc_read(uint8_t channel) { /* select channel */ ADMUX = channel; ADMUX |=(1<<REFS0); //here it is again /* start conversion */ ADCSRA |= (1 << ADSC); /* wait until conversion finished */ while (ADCSRA & (1 << ADSC)) { } /* return the result */ return ADCW; } /* 96.1 cycles per sec, 10.4ms per cycle MEASURED ON OSCILLOSCOPE*/ /* read all the ADC inputs and do some conversion */ void sample_inputs(void) {
uint16_t adc0, adc1, adc2, adc3, adc4, adc5;
accsum=0; gyrosum=0;
for (i=0; i<20; i++) {
adc0 = adc_read(0); /* accelerometer input pin PA0 (0-5V input signal)*/ adc1 = adc_read(1); /* gyro input pin PA1 (0-5V input signal)*/
accsum= (float) accsum+adc0; accsum is the sum of 20 accelerometer readings gyrosum= (float) gyrosum+adc1; gyrosum is the sum of 20 gyro output readings }
adc2 = adc_read(2); /* grey wire overallgain (via cutout switch) position PA2*/ adc3 = adc_read(3); /* Position lever pulled back position PA3*/
adc5 = adc_read(5); /* Position lever pushed forwards position PA5*/
if (tipstart<1) {
gaincontrol = (float) adc2/341; //gives range 0-3 adc4 = adc_read(4); /* Throttle pedal position (this is a control knob on handset which determines how fast it accelerates when you deliberately tilt it forwards) PA4*/
Throttle_pedal=(float) adc4/341; //gives range 0-3 }
if (adc2<100) { Throttle_pedal=0.001; gaincontrol=0.001; }
overallgain = gaincontrol*softstart; //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;
if (adc3>100) Balance_point=535; i.e. if tilt lever pushed one way on the hand controller it resets the balance point if (adc5>100) Balance_point=500; i.e. if tilt lever pushed the other way on hand controller the balance point is reset slightly the other way (easier to go up a slope with low ground clearance if front of skateboard tilted up slightly before you start). If you are starting out trying to get a machine to balance, just forget about adc3 and adc5 as I have used them and don't bother with the 3 lines of code above. PORTB |= (1<<PB2); //Port B2 turned on/off once per loop so I can measure loop time with an oscilloscope ACCELEROMETER signal processing:
/*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!
GYRO signal processing:
/*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 scaling for the "i" or integral factor (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 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 already been corrected over the longer term by the accelerometer readings. The gangleraterads term reflects the RATE of tilt (from gyro). Therefore even if it does not look like it, (as you seem to just have the two terms; anglerads and gangleraterads, you do in fact have the three P(proportional), I (integral) and D(derivative) terms all present here in the above equation. 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, 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 (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.
} Configure timer and set up the output pins OC1A(Pin PD5 on my micro) and OC1B(Pin PD4 on my micro) as phase-correct PWM output channels Note: Some strongly feel that locked-antiphase is the way to go as get regenerative braking and good control around mid-balance point Downside is that you can get a lot more noise and voltage spikes in system but these can be smoothed out with filters Others are far more expert on this than I am so need to look into this for yourself but this is my understanding. My aim is to start with phase-correct as I just about understand it and others have used it OK, then develop from there.
Note: You can also get motor controllers for robots that just require a voltage input of 0-5V with 2.5V representing no motor movement in either direction.
These may be simpler to get working than the OSMC was, although the OSMC has the advantage of handling a lot of current. For example for my smaller 2 wheeler I used a "Sabertooth" controller.
void timer_init() { TCCR0 = 0 | (1<<CS02) | (1<<CS01) | (1<<CS00); // External clock to Pin T0 Clock on rising edge/1024 // PWM mode is "PWM, Phase Correct, 10-bit"
I am grateful to Trevor Blackwell for this section, I had no idea at all what any of it meant when I started.
TCCR1A = 0 | (1<<COM1A1) | (1<<COM1A0) | // set on match up, clear on match down (1<<COM1B1) | (1<<COM1B0) | // set on match up, clear on match down (1<<WGM11) | (1<<WGM10); //OCR1_Max is 1023 so these are set like this TCCR1B = 0 | (1<<CS10); // prescaler divide by 1 see P131 datasheet about prescaling values to change here. /* 16 MHz / 1 / 1024 / 2 gives 8 kHz, probably about right */ } void set_motor() /* The leveli terms is the level term rescaled to give a value from -1023 to +1023 as an integer ready to send to the PWM motor control output pins on the microcontroller that are in turn connected to the OSMC PWM (pulse width modulated) signal inputs. The level term should be within the range -1 to +1 and buzzer should have already gone off if greater than 0.7 or less than -0.7*/ { int16_t leveli = (int16_t)(level*1023); //NOTE: level is a floating point value but here we multiply it by 1023 and also force the result into being an INTEGER before sending it to the PWM generator. if (leveli<-1020) leveli=-1020; //double-checks we are within sensible limits (on the scale of 0 - 1023) as do not want to suddenly be thrown off the board if (leveli>1020) leveli=1020;
”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); } 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.
/*NOTE: Not sure why but to stop motor cutting out on direction changes I had in the end to hard wire AHI and BHI to +12V */ /* Un-disabled OSMC by setting PinC1 output to zero, a 1 would disable the OSMC*/ PORTC |= 0x0c; //make C1 pulled down so un-disables the OSMC i.e. enables it. PORTC &= ~0x02; //disable is off if (leveli<0) { OCR1A = -leveli; // ALI is PWM we going backwards here as leveli variable is a negative signed value, keep the minus sign in here! OCR1B = 0; // BLI = 0 } else { OCR1A = 0; // ALI = 0 going forwards here as leveli variable is a positive signed value OCR1B = leveli; // BLI is PWM } } Initial tilt-start code below. The whole program actually starts here when you turn micro on. 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.
int main(void) { InitPorts(); adc_init(); timer_init(); tipstart=0; while (tipstart<1){ sample_inputs(); if (x_accdeg>0) { tipstart=0; } else { tipstart=1; softstart=0.4; } } angle=0; cur_speed=0; end of tilt-start code. If go beyond this point then machine has successfully become level and is actively balancing from this time onwards*/ sei(); while (1) { sample_inputs(); set_motor(); } } Additional notes on scaling of gyro output and use of –ve and +ve values with zero as the “balanced” point: 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 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. |