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 #include #include #include #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<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<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; This limits the rate of change of gyro angle to just a little less than the maximum rate it is actually capable of measuring (which is 100deg/sec) 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<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<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 For my micro, an input voltage of 0V gives a value of 0 in the code, while an input voltage of 5V gives a value of 1023 in the code (0 - 1023 = 1024 individual points) This is because the micro has to convert the continuous analog voltage to a digital value it then uses in the rest of the code. So 5V has to be converted to the highest value on some sort of scale, with 2.5V giving the mid point on this scale. For some reason related to computer maths binary, the scale the micros use is 0 - 1023. Some very simple micros I think use a scale of 0 - about 256 from memory. So your voltage of 0 每 5V coming into the analog input can be coverted to a value on scale of 0 每 1023 i.e. a decent number of steps so it can detect small voltage (angle) changes. The more points on the scale the smaller the voltage change it can detect. If there were only a few points, the skateboard would be jumpy as it could only detect big changes in voltage inputs from the sensors. The scale of 0 - 1023 is somehow in the setup of the analog inputs in first part of program (adapted more or less unchanged from Trevor Blackwell's code if you look at it). I don't fully understand how this setup works but it affects the resolution or number of steps in the scale so I have not messed about with it. An input of 2.5V gives a value in the code of about 512. How to adapt this software for your particular gyro voltage output per degree per second of rotation (tilting): 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. Having subtracted (about 512) from the reading, I now get a value of -512 when gyro rotating max speed anticlockwise and +512 when rotating clockwise at full speed. This just makes writing the rest of the code easier to imagine in my head: minus (-ve) values mean tilting one way and +ve values mean tilting the other way while a value of 0 means perfectly balanced (i.e. the middle point). This applies to the accelerometer as well (also a 0-5V output). 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. Hope this helps. It takes ages to work these things out then when you look back it seems so obvious. It took me ages (months) to work out what the code was doing in other peoples programs (I studied the code of the Trevor Blackwell unicycle and also "Meta" which was based on Trevor Blackwell's work). You also see lots of subtractions of 512 at various points in their code for the same reasons I have done