/********************************************************************/ /* */ /* Auto balancing platform by using 2-axis */ /* accelerometer for CUHK ARM board (8/2015) */ /* */ /********************************************************************/ /* */ /* ARM CPU: Philips LPC2138 */ /* Crystal clock: 11.0592 MHz */ /* Serial port baudrate: 57600 */ /* Programming tool: KEIL uVision4 RealView for ARM */ /* */ /********************************************************************/ #include extern void init_timer(void); #define PWM_FREQ 276000 //set PWM frequency to 50 Hz #define L_DIR 0x00010000 //set p0.16 as left motor direction #define L_DIRinv 0x00020000 //set p0.17 as inverted left motor direction #define R_DIR 0x00040000 //set p0.18 as right motor direction #define R_DIRinv 0x00080000 //set p0.19 as inverted right motor direction #define JUMPER 0x00000100 //set p0.8 as function selection jumper #define NEWLINE sendchar(0x0a); sendchar(0x0d) // Adjustable parameter ///////////////// #define INIT_POSL 25000 // Left servo initial position #define INIT_POSR 18000 // Right servo initial position #define maxaccu 200 // Maximum accumulate error #define maxdiff 2000 // Maximum different between current and last error #define MINOUTPUT 7000 // Minimum value of PWM output #define MAXOUTPUT 35000 // Maximum value of PWM output #define P 1 // Proportional constant of PID controller #define I 1 // integral constant of PID controller #define D 1 // Differential constant of PID controller #define MIDL 15500 // The set point value of accelerometer X-axis #define MIDR 15800 // The set point value of accelerometer Y-axis // Global variables ///////////////////// long leftPWM,rightPWM; // PWM values int tmpl,deltal,lastl,accul,diffl; // variables for left servo int tmpr,deltar,lastr,accur,diffr; // variables for right servo // Functions ======================================= void Init_Serial_A(void) { U0LCR = 0x83; //8 bit length ,DLAB=1 U0DLL = 0x0F; //Pclk/(16*baudrate)=(11059200 x 5)/(4 x 16 x 57600); U0DLM = 0x00; U0LCR = 0x03; //DLAB=0 } char getchar(void) { volatile char ch = '0'; while ((U0LSR&0x1)==0) // wait until receive a byte ; ch = U0RBR; // receive character return ch; } void sendchar(char ch) { while( (U0LSR&0x40)==0 ); U0THR = ch; // Transmit next character } int print(char *p) { while(*p!='\0') { sendchar(*p++); } return(0); } void putint(int count) { sendchar('0' + count/10000); sendchar('0' + (count/1000) % 10); sendchar('0' + (count/100) % 10); sendchar('0' + (count/10) % 10); sendchar('0' + count % 10); } int read_sensor(int channel) { int temp; ADCR=0x1<>=6; //ADDR p6-p15 = RESULT temp&=0x3ff;//10 bits result return (temp*33); //return 0-3V value precision is 1024 } //================================================ int main(void) { long tmpjp; PINSEL0 = 0x00000005; // set p0.0 to TXD0, p0.1 to RXD0 and the rest to GPIO PINSEL1 = 0x00400000; // set p0.27 to ad0.0 and the rest to GPIO PINSEL1 |= 0x01000000; // set p0.28 to ad0.1 PINSEL0 |= 0x00008000; // set p0.7 to PWM2 PINSEL0 |= 0x00080000; // set p0.9 to PWM6 Init_Serial_A(); // Init COM port NEWLINE; print("======================================================================="); NEWLINE; print("* *"); NEWLINE; print("* CUHK Computer Science and Engineering Department *"); NEWLINE; print("* LPC2138 ARM Board *"); NEWLINE; print("* *"); NEWLINE; print("* Auto balancing platform by using 2-axis accelerometer *"); NEWLINE; print("* (8/2015) *"); NEWLINE; print("* *"); NEWLINE; print("======================================================================="); NEWLINE; NEWLINE; tmpjp = IO0PIN & JUMPER; // check function selection jumper if(tmpjp==0) { // if jumper is set then print X, Y value while(1) { print("Xaxis = "); putint(read_sensor(0)); print(" ; Yaxis = "); putint(read_sensor(1)); NEWLINE; NEWLINE; print("Press any key to show X and Y value."); NEWLINE; NEWLINE; getchar(); } } else { // else run self balancing demo init_timer(); // Init TIMER 0 PWMPCR=0x4000; // enable pwm6 PWMPCR|=0x0400; // enable pwm2 PWMMCR=0x0002; PWMMR0 = PWM_FREQ; //set PWM frequency to 50 Hz //set robot to full speed leftPWM=INIT_POSL; rightPWM=INIT_POSR; PWMMR2 = leftPWM; //set left motor PWM width to full speed PWMMR6 = rightPWM; //set right motor PWM width to full speed PWMLER = 0x45; //enable match 0,2,5 latch to effective PWMTCR=0x09; //set p0.16-p0.19 as output IO0DIR|=L_DIR; IO0DIR|=L_DIRinv; IO0DIR|=R_DIR; IO0DIR|=R_DIRinv; // enable PWM outputs IO0SET|=L_DIR; IO0CLR|=L_DIRinv; IO0SET|=R_DIRinv; IO0CLR|=R_DIR; // initialize variables lastl=0; accul=0; diffl=0; lastr=0; accur=0; diffr=0; while(1) { } } } void __irq IRQ_Exception() { tmpl = read_sensor(0); // read X-axis value if (tmpl>=(MIDL+200)) { // if X-axis value >= setpoint plus 200 deltal = (tmpl - (MIDL+200))/200; // calculate the error and normalize it diffl = deltal-lastl; // caculate the different between current and last error if(diffl max. difference // this prevent the noise due to undesired movement of accelerometer lastl = deltal; // save error as the last error leftPWM = leftPWM - (P*deltal - I*accul + D*diffl); // update the left PWM value by PID if (leftPWM max. difference // this prevent the noise due to undesired movement of accelerometer lastl = deltal; // save error to the last error leftPWM = leftPWM + P*deltal + I*accul + D*diffl; // update the left PWM value by PID if (leftPWM>MAXOUTPUT) leftPWM = MAXOUTPUT; // limit the PWM value to its maximum if(accul>0) accul -= deltal/200; // ensure the integral not less than zero PWMMR2=leftPWM; // set the left PWM output PWMLER = 0x44; //enable match 2,6 latch to effective } } //////////////////////////////////////////////////// tmpr = read_sensor(1); // read Y-axis value if (tmpr>=(MIDR+200)) { // if Y-axis value >= setpoint plus 200 deltar = (tmpr - (MIDR+200))/200; // calculate the error and normalize it diffr = deltar-lastr; // caculate the different between current and last error if(diffr max. difference // this prevent the noise due to undesired movement of accelerometer lastr = deltar; // save error as the last error rightPWM = rightPWM - (P*deltar - I*accur + D*diffr); // update the right PWM value by PID if (rightPWM max. difference // this prevent the noise due to undesired movement of accelerometer lastr = deltar; // save error as the last error rightPWM = rightPWM + P*deltar + I*accur + D*diffr; // update the right PWM value by PID if (rightPWM>MAXOUTPUT) rightPWM = MAXOUTPUT; // limit the PWM value to its maximum if(accur>0) accur -= deltar/200; // ensure the integral not exceed the maximum PWMMR6=rightPWM; // set the right PWM output PWMLER = 0x44; //enable match 2,6 latch to effective } } ////////////////////////////////////////////////////////// T0IR = 1; // Clear interrupt flag VICVectAddr = 0; // Acknowledge Interrupt } /* Setup the Timer Counter 0 Interrupt */ void init_timer (void) { T0PR = 0; // set prescaler to 0 T0MR0 = 27648; // set interrupt interval to 1mS // Pclk/500Hz = (11059200 x 5)/(4 x 1000) T0MCR = 3; // Interrupt and Reset on MR0 T0TCR = 1; // Timer0 Enable VICVectAddr0 = (unsigned long)IRQ_Exception; // set interrupt vector in 0 VICVectCntl0 = 0x20 | 4; // use it for Timer 0 Interrupt VICIntEnable = 0x00000010; // Enable Timer0 Interrupt }