Modify a code from an ATmega32 to an Arduino Mega

This code is for a self balancing skateboard.

Page 1 of 1

5 Replies - 5281 Views - Last Post: 03 January 2010 - 07:04 AM Rate Topic: -----

#1 Tholsadum   User is offline

  • New D.I.C Head

Reputation: 0
  • View blog
  • Posts: 13
  • Joined: 29-December 09

Modify a code from an ATmega32 to an Arduino Mega

Posted 29 December 2009 - 07:25 AM

Hi!

I'm trying to modify this code in an attempt to make it work on an Arduino Mega. I'm pretty much new to C so, I may have made some major mistakes. By the way, this is for a self balancing skateboard. :P

This code is taken from an ATmega32 (from : http://sites.google....t-version/code4 and I'm trying to make it work on a Arduino Mega.


This code was writen for an ATmega32 developpement board
http://www.active-ro...llr/m32db.shtml

Thank you!

Here is the first error I encounter :

 In function 'void sample_inputs()':
error: '92' cannot be used as a function In function 'void timer_init()':
 In function 'int main()':


Could someone explain me what is wrong?
I'm pretty much a beginner in programming but I've read a lot of books/website and I'm learning fast too! ^^
and here is the complete code (its pretty long):

#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 accelraw;
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.0064;
float Balance_point;
float a0, a1, a2, a3, a4, a5, a6;//Savitzky-Golay variables for accelerometer


int i;
int j;
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
//PORTC |= (1<<PC1); //make C1 +ve so disables OSMC during startup

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
 PC4		Onboard LED
	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
	PA6/ADC6   Vbatt/10 -> OSMC pin 3
	PA1/ADC1   pitch rate gyro
	PA0/ADC0   accelerometer
 */
 
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;
}

/* 156 cycles per sec, 6.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;
  
  gyrosum=0;
  adc0 = adc_read(0); /* accelerometer pin PA0 */
  accelraw = (float) adc0;
 
  for (j=0; j<7; j++) {
	 
  adc1 = adc_read(1);	  //gyro pin PA1		 
  gyrosum = (float) gyrosum + adc1; //using a mean of 7 samples per loop for the gyro so it gets a complete update with each loop of the program
						  }

  
						
  
 
  adc2 = adc_read(2); /* grey wire overallgain (via cutout switch) position PA2*/
  adc3 = adc_read(3); /* Position lever pulled back position PA3*/
  adc4 = adc_read(4); /* Throttle_pedal position PA4*/
  adc5 = adc_read(5); /* Position lever pushed forwards position PA5*/
  //adc6 = adc_read(6); /* Vbatt input from OSMC (not used at present) position PA6*/
   
  
//Sav Golay filter for accel only
  a0 = a1;
  a1 = a2;
  a2 = a3;
  a3 = a4;
  a4 = a5;
  a5 = a6;
  a6 = (float) accelraw;
  accsum = (float) ((-2*a0) + (3*a1) + (6*a2) + (7*a3) + (6*a4) + (3*a5) + (-2*a6))/21;  //Sav Golay calculation
 
 
 
	  
	  gaincontrol = (float) gaincontrol*0.9 + 0.1*adc2/341; //smooths any voltage spikes and gives range 0-3
   
	  Throttle_pedal=(float) Throttle_pedal*0.9 + 0.1*adc4/341; //smooths any voltage spikes and gives range 0-3

  
//Cuts the motor if the dead mans button is let go 
//(gaincontrol variable also wired in through this button to adc2
  if (adc2<100) {
		Throttle_pedal=0.001;
		gaincontrol=0.001;
		  }
  
  overallgain = gaincontrol*softstart;
 
 
//what to do if lever pulled back or pushed forwards or not doing anything:
   Balance_point = 514;
   
   if (adc3>100) Balance_point=534;

   if (adc5>100) Balance_point=494;
 

	 
  
  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 - Balance_point; //accsum is SG value for accelerometer, not a true "sum" so no need to divide by 7
  
  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*/

   x_accdeg= (float) x_acc/-3.45; //The minus sign corrects for a back to front accelerometer mounting!
 
 
 
	/*GYRO signal processing*/
  
  /*Subtract offsets: Sensor reading is 0-1024 so "balance point" i.e. my required zero point will be that reading minus 512*/
	  
 

  /*Gyro angle change of 20mV per deg per sec from datasheet gives change of 4.096 units (on the scale of 0 - 1023) per degree per sec angle change
	This limits the rate of change of gyro angle to just less than the maximum rate it is actually capable of measuring (100deg/sec). Note all these fractions are rounded up to an integer later just before it is sent to the PWM generator which in turn is connected to the motor controller*/
 
 
gangleratedeg=(float)((gyrosum/7) - 508)/4.096; //gyrosum is a sum of a group of 7 samples so divide by 7 for gyro value
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 2.2 here)
	gyroangledt is anglechange since last CYCLE in degrees from gyro sensor, where ti is scaling factor (should in theory be about 1 but 2.2 makes board feel tighter)
 
	ganglerate is now in units of degrees per second
	aa varies the time constant, i.e smaller aa value makes accelerometer time constant longer as it slowly corrects for the gyro drift*/

   aa=0.005;
  
   gyroangledt = (float)ti*cycle_time*gangleratedeg;
   gangleraterads=(float)gangleratedeg*0.017453;
  

   /*new angle in DEGREES is old angle plus change in angle from gyro since last cycle with little bit of new accel reading factored in*/
   angle = (float)((1-aa) * (angle+gyroangledt)) + (aa * x_accdeg); //the main angle calculating function*/
 
//Convert angle from degrees to radians

 
   anglerads=(float)angle*0.017453;
  
	
	balance_torque=(float)(4.5*anglerads) + (0.5*gangleraterads);
	
 

 

 

cur_speed = (float)(cur_speed + (Throttle_pedal * balance_torque * cycle_time)) * 0.999;

/*The level value is from -1 to +1 and represents the duty cycle to be sent to the motor. Converting to radians helps us stay within these limits
 
 
level = (balance_torque + cur_speed) * overallgain;

 

}

 

 

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"
  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.
 /* 1[/

void set_motor()

/* The leveli terms is the level term rescaled from -1023 to +1023 as an integer ready to send to the PWM motor control ports that are in turn connected to the OSMC*/
{


  //if (level<-0.9) level= -0.9;//checks we are within sensible limits
  //if (level>0.9) level=0.9;

  int16_t leveli = (int16_t)(level*1023); //NOTE here we take the floating point value we have ended up with for "level", we multiply it by 1023 and then make it into an integer before feeding the value into the PWM generator as "leveli"

  if (leveli<-1020) leveli=-1020;//double-checks we are within sensible PWM limits as do not want to suddenly be thrown off the board
  if (leveli>1020) leveli=1020;
 


/*Set up LED or buzzer on Port B1 to warn me to slow down if torque to be delivered is more than 50% 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 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;


 
  //PORTC |= (0<<PC1);	 // AHI=1  PinC3, BHI=1 PinC2 set both to ON for OSMC to work and both to OFF to shut motor down
/*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   going backwards 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 as leveli variable is a positive signed value
	OCR1B = leveli;	// BLI is PWM
  }
 
}


int main(void)
{
  
	InitPorts();

 adc_init();

 timer_init();


/*  Initial tilt-start code
 Turn on micro while board tipped to one side,
 rider about to step onto it, if tilt angle crosses zero (mid) point balance algorithm 
 becomes operational otherwise locked in this loop forever until it is tipped to level position as rider 
 gets onto board*/
	tipstart=0;
 accelraw = 0;

	while (tipstart<1){

// you need this to allow the SG filter to wind up to the proper stable value when you first turn machine on, before looking at the value of accsum (below).

	 for (i=0; i<20; i++) {
	  sample_inputs();		   
				  }

 

 if (accsum<504 || accsum>524) {
 //   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 become level and is active*/


	sei();

 
 while (1) {
 
 sample_inputs();

 set_motor();
 

	}
}



Thank you a lot! :P

This post has been edited by Tholsadum: 29 December 2009 - 07:51 AM


Is This A Good Question/Topic? 0
  • +

Replies To: Modify a code from an ATmega32 to an Arduino Mega

#2 NickDMax   User is offline

  • Can grep dead trees!
  • member icon

Reputation: 2255
  • View blog
  • Posts: 9,245
  • Joined: 18-February 07

Re: Modify a code from an ATmega32 to an Arduino Mega

Posted 29 December 2009 - 07:42 AM

if (gangleratedeg >92) gangleratedeg=92 <--- missing semicolon...
Was This Post Helpful? 1
  • +
  • -

#3 Tholsadum   User is offline

  • New D.I.C Head

Reputation: 0
  • View blog
  • Posts: 13
  • Joined: 29-December 09

Re: Modify a code from an ATmega32 to an Arduino Mega

Posted 29 December 2009 - 07:53 AM

Thanks a lot NickDMax. :D

I've corrected the missing ; !

But now here is what appears:

In function 'void timer_init()':
error: 'TCCR0' was not declared in this scope In function 'int main()':


Any ideas?

This post has been edited by Tholsadum: 29 December 2009 - 07:55 AM

Was This Post Helpful? 0
  • +
  • -

#4 janotte   User is offline

  • code > sword
  • member icon

Reputation: 991
  • View blog
  • Posts: 5,141
  • Joined: 28-September 06

Re: Modify a code from an ATmega32 to an Arduino Mega

Posted 29 December 2009 - 07:56 AM

View PostTholsadum, on 29 Dec, 2009 - 06:53 AM, said:

In function 'void timer_init()':
error: 'TCCR0' was not declared in this scope In function 'int main()':


Any ideas?


You use TCCR0 = 0 | as the first line in the function void timer_init().

Where did you declare it?
Was This Post Helpful? 0
  • +
  • -

#5 Tholsadum   User is offline

  • New D.I.C Head

Reputation: 0
  • View blog
  • Posts: 13
  • Joined: 29-December 09

Re: Modify a code from an ATmega32 to an Arduino Mega

Posted 29 December 2009 - 08:06 AM

I should have had:

float TCCR0;


at the beginning?

This post has been edited by Tholsadum: 29 December 2009 - 08:07 AM

Was This Post Helpful? 0
  • +
  • -

#6 Tholsadum   User is offline

  • New D.I.C Head

Reputation: 0
  • View blog
  • Posts: 13
  • Joined: 29-December 09

Re: Modify a code from an ATmega32 to an Arduino Mega

Posted 03 January 2010 - 07:04 AM

I've done some modifications but now encounter this:

o: In function `main':
C:\Users\*******\AppData\Local\Temp\build27006.tmp/Test2.cpp:406: undefined reference to `setup'



BTW, I'm using the arduino compiler (you can find it here )
#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 accelraw;
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.0064;
float Balance_point;
float a0, a1, a2, a3, a4, a5, a6;//Savitzky-Golay variables for accelerometer

float TCCR0;

int i;
int j;
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
//PORTC |= (1<<PC1); //make C1 +ve so disables OSMC during startup

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
 PC4		Onboard LED
	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
	PA6/ADC6   Vbatt/10 -> OSMC pin 3
	PA1/ADC1   pitch rate gyro
	PA0/ADC0   accelerometer
 */
 
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;
}

/* 156 cycles per sec, 6.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;
  
  gyrosum=0;
  adc0 = adc_read(0); /* accelerometer pin PA0 */
  accelraw = (float) adc0;
 
  for (j=0; j<7; j++) {
	 
  adc1 = adc_read(1);	  //gyro pin PA1		 
  gyrosum = (float) gyrosum + adc1; //using a mean of 7 samples per loop for the gyro so it gets a complete update with each loop of the program
						  }

  
						
  
 
  adc2 = adc_read(2); /* grey wire overallgain (via cutout switch) position PA2*/
  adc3 = adc_read(3); /* Position lever pulled back position PA3*/
  adc4 = adc_read(4); /* Throttle_pedal position PA4*/
  adc5 = adc_read(5); /* Position lever pushed forwards position PA5*/
  //adc6 = adc_read(6); /* Vbatt input from OSMC (not used at present) position PA6*/
   
  
//Sav Golay filter for accel only
  a0 = a1;
  a1 = a2;
  a2 = a3;
  a3 = a4;
  a4 = a5;
  a5 = a6;
  a6 = (float) accelraw;
  accsum = (float) ((-2*a0) + (3*a1) + (6*a2) + (7*a3) + (6*a4) + (3*a5) + (-2*a6))/21;  //Sav Golay calculation
 
 
 
	  
	  gaincontrol = (float) gaincontrol*0.9 + 0.1*adc2/341; //smooths any voltage spikes and gives range 0-3
   
	  Throttle_pedal=(float) Throttle_pedal*0.9 + 0.1*adc4/341; //smooths any voltage spikes and gives range 0-3

  
//Cuts the motor if the dead mans button is let go 
//(gaincontrol variable also wired in through this button to adc2
  if (adc2<100) {
		Throttle_pedal=0.001;
		gaincontrol=0.001;
		  }
  
  overallgain = gaincontrol*softstart;
 
 
//what to do if lever pulled back or pushed forwards or not doing anything:
   Balance_point = 514;
   
   if (adc3>100) Balance_point=534;

   if (adc5>100) Balance_point=494;
 

	 
  
  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 - Balance_point; //accsum is SG value for accelerometer, not a true "sum" so no need to divide by 7
  
  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*/

   x_accdeg= (float) x_acc/-3.45; //The minus sign corrects for a back to front accelerometer mounting!
 
 
 
	/*GYRO signal processing*/
  
  /*Subtract offsets: Sensor reading is 0-1024 so "balance point" i.e. my required zero point will be that reading minus 512*/
	  
 

  /*Gyro angle change of 20mV per deg per sec from datasheet gives change of 4.096 units (on the scale of 0 - 1023) per degree per sec angle change
	This limits the rate of change of gyro angle to just less than the maximum rate it is actually capable of measuring (100deg/sec). Note all these fractions are rounded up to an integer later just before it is sent to the PWM generator which in turn is connected to the motor controller*/
 
 
gangleratedeg=(float)((gyrosum/7) - 508)/4.096; //gyrosum is a sum of a group of 7 samples so divide by 7 for gyro value
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 2.2 here)
	gyroangledt is anglechange since last CYCLE in degrees from gyro sensor, where ti is scaling factor (should in theory be about 1 but 2.2 makes board feel tighter)
 
	ganglerate is now in units of degrees per second
	aa varies the time constant, i.e smaller aa value makes accelerometer time constant longer as it slowly corrects for the gyro drift*/

   aa=0.005;
  
   gyroangledt = (float)ti*cycle_time*gangleratedeg;
   gangleraterads=(float)gangleratedeg*0.017453;
  

   /*new angle in DEGREES is old angle plus change in angle from gyro since last cycle with little bit of new accel reading factored in*/
   angle = (float)((1-aa) * (angle+gyroangledt)) + (aa * x_accdeg); //the main angle calculating function*/
 
//Convert angle from degrees to radians

 
   anglerads=(float)angle*0.017453;
  
	
	balance_torque=(float)(4.5*anglerads) + (0.5*gangleraterads);
	
 

 

 

cur_speed = (float)(cur_speed + (Throttle_pedal * balance_torque * cycle_time)) * 0.999;

/*The level value is from -1 to +1 and represents the duty cycle to be sent to the motor. Converting to radians helps us stay within these limits*/
 
 
level = (balance_torque + cur_speed) * overallgain;

 

}

 

 

/* 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 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*/

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"
  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 from -1023 to +1023 as an integer ready to send to the PWM motor control ports that are in turn connected to the OSMC*/
{


  //if (level<-0.9) level= -0.9;//checks we are within sensible limits
  //if (level>0.9) level=0.9;

  int16_t leveli = (int16_t)(level*1023); //NOTE here we take the floating point value we have ended up with for "level", we multiply it by 1023 and then make it into an integer before feeding the value into the PWM generator as "leveli"

  if (leveli<-1020) leveli=-1020;//double-checks we are within sensible PWM limits as do not want to suddenly be thrown off the board
  if (leveli>1020) leveli=1020;
 


/*Set up LED or buzzer on Port B1 to warn me to slow down if torque to be delivered is more than 50% 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 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;


 
  //PORTC |= (0<<PC1);	 // AHI=1  PinC3, BHI=1 PinC2 set both to ON for OSMC to work and both to OFF to shut motor down
/*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   going backwards 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 as leveli variable is a positive signed value
	OCR1B = leveli;	// BLI is PWM
  }
 
}


void loop()
{
  
	InitPorts();

 adc_init();

 timer_init();


/*  Initial tilt-start code
 Turn on micro while board tipped to one side,
 rider about to step onto it, if tilt angle crosses zero (mid) point balance algorithm 
 becomes operational otherwise locked in this loop forever until it is tipped to level position as rider 
 gets onto board*/
	tipstart=0;
 accelraw = 0;

	while (tipstart<1){

// you need this to allow the SG filter to wind up to the proper stable value when you first turn machine on, before looking at the value of accsum (below).

	 for (i=0; i<20; i++) {
		sample_inputs();			   
						  }

 

 if (accsum<504 || accsum>524) {
 //   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 become level and is active*/


	sei();

 
 while (1) {
 
 sample_inputs();

 set_motor();
 

	}
}

Was This Post Helpful? 0
  • +
  • -

Page 1 of 1