// ECE 476 Final Projec: Autonomous stability system for a co-axial Helicopter
// Note: This code has been edited for better
//created Friday, April 7th
// last updated april 21st
// last updated april 23rd
// last updated May 3rd
#include <Mega32.h>
#include <math.h>
// MATH STUFF
// Code Credit for fixed point code: Bruce Land http://www.nbb.cornell.edu/neurobio/land/
//****************************************************************************************
//===The fixed macros=========================================
#define int2fix(a) (((int)(a))<<8) //Convert char to fix
#define fix2intSlow(a) ((signed char)((a)>>8)) //Convert fix to char
#define float2fix(a) ((int)((a)*256.0)) //Convert float to fix
#define fix2float(a) ((float)(a)/256.0) //Convert fix to float
#define multfixSlow(a,b) ((int)((((long)(a))*((long)(b)))>>8)) //multiply two fixed #
#define divfix(a,b) ((int)((((long)(a))<<8)/((long)(b)))) //divide two fixed #
//lsqrt is in math.h
#define sqrtfixSlow(a) (lsqrt(((long)(a))<<8)) //square root
//==Fast fixed multiply=================================
char fix2int(int a)
{
#asm
ldd r30, Y+1 ;moves the high byte of the input to low byte
#endasm
}
//==Fast fixed multiply=================================
int multfix(int a,int b)
{
#asm
;******************************************************************************
;*
;* FUNCTION
;* muls16x16_24
;* DECRIPTION
;* Signed multiply of two 16bits numbers with 24bits result.
;* USAGE
;* r31:r30:rxx = r23:r22 * r21:r20
;******************************************************************************
push r20
push r21
LDD R22,Y+2 ;load a
LDD R23,Y+3
LD R20,Y ;load b
LDD R21,Y+1
muls r23, r21 ; (signed)ah * (signed)bh
mov r31, r0 ;r18, r0
mul r22, r20 ; al * bl
mov r30, r1 ;movw r17:r16, r1:r0
;mov r16, r0
mulsu r23, r20 ; (signed)ah * bl
add r30, r0 ;r17, r0
adc r31, r1 ;r18, r1
mulsu r21, r22 ; (signed)bh * al
add r30, r0 ;r17, r0
adc r31, r1 ;r18, r1
pop r21
pop r20
#endasm
}
// End fixed point math code
// *********************************
#define minimum 0 // minimum PWM value
#define maxticks 100 // used for PWM
#define maximum 100 // maximum PWM value
#define Smax 60 // Maximum PWM value for stability motors
#define Sinc 256 // Stability Motor PWM increment
#define milsec 160 // used for our "seconds" counter used for
// helicopter timing
// Define sensor channels
#define GYRO_CHANNEL 0
#define AROMX_CHANNEL 1
#define AROMY_CHANNEL 2
#define AROMZ_CHANNEL 3
// Define Helicopter states
#define GROUND 0
#define LANDING 1
#define TAKEOFF 2
#define HOVER 3
#define LEFT 4
#define RIGHT 5
#define FORWARD 6
#define BACKWARD 7
// start off on the ground if we live in reality
char heliState = GROUND;
//used for timing of PWM
int ticks = 0;
char marks = 0;
int msec = 0;
int sec = 0;
// make motor speeds fixed point for greater accuracy
int topRotorSpeed;
int bottomRotorSpeed;
int motorStable1 = 0;
int motorStable2 = 0;
int motorStable3 = 0;
// used sometimes
int avgRotorSpeed = int2fix(0);
int i = 0;
char j,k;
char temp;
// ADC variables
char Ain = 0;
char Ainlast = 0;
char first = 0;
// Gyroscope
char gyroCalibrate = 0; // calibration
char gyro[32]; // buffer
char lastG = 0; // not used
// X accelerometer
char aromXCalibrate = 0; // calibration
char aromX[128]; // buffer
char flatnessX = 0; // not used
// Y accelerometer
char aromYCalibrate = 0; // calibration
char aromY[128]; // buffer
char flatnessY = 0; // not used
// Z accelerometer
char aromZCalibrate = 0; // calibration
char aromZ[128]; // buffer
char currentChannel = 0;
void initialize(void);
void updatePWM(void);
void updateADC(void);
void calibrateGyro(void);
void calibrateArom(void);
char FixFilter32(char f[32]);
char FixFilter64(char f[64]);
char FixFilter128(char f[128]);
char FixFilter256(char f[256]);
char aromZfilter(char f[256]);
interrupt [TIM0_COMP] void pwm(void){
ticks++;
marks++;
if(marks == milsec){
marks = 0;
msec++;
if(msec == 10){
msec = 0;
sec++;
}
}
updatePWM();
}
void main(void){
initialize();
// wait for button press on 1
while(PIND.1 != 0) { }
calibrateGyro();
calibrateArom();
// Lets a GO!
topRotorSpeed = avgRotorSpeed;
bottomRotorSpeed = avgRotorSpeed;
sec = 0;
while(1)
{
if(ADCSR.6 == 0) updateADC();
// take off
// increase the motor speeds quickly but not abruptly
// so we dont screw up the gears
if(sec <= 5){
heliState = TAKEOFF;
if(topRotorSpeed < int2fix(95)) topRotorSpeed += 16;
if(bottomRotorSpeed < int2fix(95)) bottomRotorSpeed += 16;
}
// hover
// We turn on the X, Y, Z accelerometer stability systems
if(sec == 23) {
heliState = HOVER;
}
// landing
// We turn off all the stability accelerometers
// and their respective motors for stable landing
if(sec == 299){
heliState = LANDING;
motorStable1 = 0;
motorStable2 = 0;
motorStable3 = 0;
}
if(sec >= 300 && sec <= 400){
if(topRotorSpeed > int2fix(50))topRotorSpeed -= 1;
if(bottomRotorSpeed > int2fix(50))bottomRotorSpeed -= 1;
}
// grounded
// We're done! Lets get into an infinite loop so
// watchdog resets the MCU and is ready for the next flight
if(sec >= 410){
heliState = GROUND;
WDTCR=0x08; //enable watchdog
while(1); //just wait for watchdog to reset machine
}
}
}
void updatePWM(void){
int tempTick;
tempTick = int2fix(ticks);
// PWM top motor
if (tempTick > topRotorSpeed) PORTC.0 = 0;
else PORTC.0 = 1;
// PWM second motor
if (tempTick > bottomRotorSpeed) PORTC.1 = 0;
else PORTC.1 = 1;
// front motor (1)
if(ticks > fix2int(motorStable1)) PORTC.2 = 0;
else PORTC.2 = 1;
// right motor (2)
if(tempTick > motorStable2) PORTC.3 = 0;
else PORTC.3 = 1;
// left motor (3)
if(tempTick > motorStable3) PORTC.4 = 0;
else PORTC.4 = 1;
//PWM reset etc
if (ticks == maxticks){
ticks = 0;
}
}
void updateADC(void){
Ainlast = Ain;
Ain = ADCH;
switch(currentChannel){
// currently looking at the gyro channel
// clockwise : Ain greater than gyroCalibrate
// counter-clockwise : Ain is less than gyro calibrate
// for clockwise : top less, bottom more
// for counterclockwise : top more, bottom less
// use of dead zone of +/- 1 from calibrate value
case GYRO_CHANNEL:
currentChannel = AROMX_CHANNEL;
ADMUX = 0b01100001;
// new conversion
ADCSR.6 = 1;
// we decided to compensate for gyroscopic motion
// during takeoff too
//if(heliState != TAKEOFF){
for(i = 0;i < 31; i++){
gyro[i] = gyro[i+1];
}
gyro[31] = Ain;
temp = FixFilter32(gyro);
// originally used different accuracy calibrations
// for take off and hover, and this usually helps somewhat
if(heliState == TAKEOFF) j = 8;
else j = 8;
// we had to use a small threshold since the gyro was a little too sensitive
if(temp < (gyroCalibrate - 1)) {
if(topRotorSpeed < int2fix(maximum)) topRotorSpeed += j;
if(bottomRotorSpeed > int2fix(minimum)) bottomRotorSpeed -= j;
PORTD.7 = 0;
} else if(temp > (gyroCalibrate + 1)) {
if(topRotorSpeed > int2fix(minimum)) topRotorSpeed -= j;
if(bottomRotorSpeed < int2fix(maximum)) bottomRotorSpeed += j;
PORTD.7 = 0;
} else PORTD.7 = 1;
//}
break;
// currently looking at the X axis accelerometer channel
case AROMX_CHANNEL:
currentChannel = AROMY_CHANNEL;
ADMUX = 0b01100010;
// new conversion
ADCSR.6 = 1;
if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromX[i] = aromX[i+1];
}
aromX[127] = Ain;
temp = FixFilter128(aromX);
if(temp < (aromXCalibrate)) {
flatnessX = aromXCalibrate - temp;
if(motorStable1 > 0) motorStable1 -= Sinc*2;
if(motorStable2 < int2fix(Smax)) motorStable2 += Sinc*2;
if(motorStable3 < int2fix(Smax)) motorStable3 += Sinc*2;
//PORTD.7 = 0;
} else if(temp > (aromXCalibrate + 1)) {
flatnessX = temp - aromXCalibrate;
if(motorStable1 < int2fix(Smax)) motorStable1 += Sinc*2;
if(motorStable2 > 0) motorStable2 -= Sinc*2;
if(motorStable3 > 0) motorStable3 -= Sinc*2;
//PORTD.7 = 0;
} else {
if(motorStable1 > 0) motorStable1 -= Sinc*2;
flatnessX = 0;
//PORTD.7 = 1;
}
}
break;
// currently looking at the Y axis accelerometer channel
case AROMY_CHANNEL: currentChannel = AROMZ_CHANNEL;
ADMUX = 0b01100011;
// new conversion
ADCSR.6 = 1;
if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromY[i] = aromY[i+1];
}
aromY[127] = Ain;
temp = FixFilter128(aromY);
if(temp < (aromYCalibrate)) {
flatnessY = aromYCalibrate - temp;
if(motorStable2 < int2fix(Smax)) motorStable2 += Sinc*4;
if(motorStable3 > 0 ) motorStable3 -= Sinc*2;
//PORTD.7 = 0;
} else if(temp > (aromYCalibrate)) {
flatnessY = temp - aromYCalibrate;
if(motorStable2 > 0 ) motorStable2 -= Sinc*2;
if(motorStable3 < int2fix(Smax)) motorStable3 += Sinc*4;
//PORTD.7 = 0;
} else {
if(motorStable2 > 0) motorStable2 -= Sinc;
if(motorStable3 > 0) motorStable3 -= Sinc;
flatnessY = 0;
//PORTD.7 = 1;
}
}
break;
// currently looking at the Z axis accelerometer channel
// when going up we have less g so would be lower
// when going down we have more g so should be over
case AROMZ_CHANNEL: currentChannel = GYRO_CHANNEL;
ADMUX = 0b01100000;
// new conversion
ADCSR.6 = 1;
if(heliState == HOVER){
for(i = 0;i < 127; i++){
aromZ[i] = aromZ[i+1];
}
aromZ[127] = Ain;
temp = aromZfilter(aromZ);
// this sensor was a little touchy as one direction was more sensitive
// and noisier than the other. The threshold allowed us to help this as well
// as prevent the helicopter from losing altitude which was more important than it
// gaining too much altitude
if(temp > (aromZCalibrate + 3)){
if(topRotorSpeed > int2fix(minimum)) topRotorSpeed -= 2;
if(bottomRotorSpeed > int2fix(minimum)) bottomRotorSpeed -= 2;
PORTD.7 = 0;
} else if(temp == (aromZCalibrate)){
if(topRotorSpeed < int2fix(maximum - 3)) topRotorSpeed += 5;
if(bottomRotorSpeed < int2fix(maximum - 3)) bottomRotorSpeed += 5;
PORTD.7 = 0;
} //else PORTD.7 = 1;
}
break;
}
}
void calibrateGyro(void){
//init the A to D converter
//channel zero/ left adj /EXTERNAL Aref
//!!!CONNECT Aref jumper!!!!
// choose AREF = 5
// A0 is the gyro
ADMUX = 0b01100000;
//enable ADC and set prescaler to 1/64*16MHz=250,000
//and clear interupt enable
//and start a conversion
ADCSR = 0b11000110;
for(i = 0; i < 31; i++){
while(ADCSR.6 == 1){ }; // do a bunch of conversions
gyro[i] = ADCH;
ADCSR.6 = 1;
}
while(ADCSR.6 == 1){ }; // take the last one
gyro[31] = ADCH;
gyroCalibrate = FixFilter32(gyro);
// get ready for accelerometer calibrate
ADMUX = 0b01100001;
ADCSR.6 = 1;
}
void calibrateArom(void){
// A1 is the AcceleROMeterX (AROMX)
for(i = 0; i < 127; i++){
// do a bunch of conversions
while(ADCSR.6 == 1){ };
aromX[i] = ADCH;
ADCSR.6 = 1;
}
while(ADCSR.6 == 1){ }; // take the last one
aromX[127] = ADCH;
aromXCalibrate = FixFilter128(aromX);
// get ready for next axis
ADMUX = 0b01100010;
ADCSR.6 = 1;
//A2 is AcceleROMeterY (AROMY)
for(i = 0; i < 127; i++){
// do a bunch of conversions
while(ADCSR.6 == 1){};
aromY[i] = ADCH;
ADCSR.6 = 1;
}
while(ADCSR.6 == 1){ }; // take the last one
aromY[127] = ADCH;
aromYCalibrate = FixFilter128(aromY);
// reset for operation
ADMUX = 0b01100011;
ADCSR.6 = 1;
// A3 is AcceleROMeterZ (AROMZ)
for(i = 0; i < 127; i++){
// do a bunch of conversions
while(ADCSR.6 == 1){ };
aromZ[i] = ADCH;
ADCSR.6 = 1;
}
while(ADCSR.6 == 1){ }; // take the last one
aromZ[127] = ADCH;
aromZCalibrate = FixFilter128(aromZ);
// reset for operation
ADMUX = 0b01100000;
ADCSR.6 = 1;
}
// we coded a number of different filters while we were playing around
// with different filter buffer sizes
char aromZfilter(char f[256]){
int sum = 0;
for(i = 0; i < 256; i++){
sum = sum + f[i];
}
// divide by 256
sum = sum >> 8;
return sum;
}
char FixFilter32(char f[32]){
int sum = 0;
for(i = 0; i < 32; i++){
sum = sum + f[i];
}
// divide by 32
sum = sum >> 5;
return sum;
}
char FixFilter64(char f[64]){
int sum = 0;
for(i = 0; i < 64; i++){
sum = sum + f[i];
}
// divide by 64
sum = sum >> 6;
return sum;
}
char FixFilter128(char f[128]){
int sum = 0;
for(i = 0; i < 128; i++){
sum = sum + f[i];
}
// divide by 128
sum = sum >> 7;
return sum;
}
char FixFilter256(char f[256]){
int sum = 0;
for(i = 0; i < 256; i++){
sum = sum + f[i];
}
// divide by 256
sum = sum >> 8;
return sum;
}
void initialize(void){
// port A input (to ADC)
// port C output to optoisolators
DDRC = 0xff;
// port D all input except for D7
DDRD = 0b10000000;
PORTD = 0xff;
// set up timer0
// we use this to make our own PWM with
// as many channels as we want
TIMSK = 0b00000010;
OCR0 = 100;
TCCR0 = 0b01001001;
#asm
sei
#endasm
}