Jump to content
43oh

MSP-EXP430 : Accelerometer and Servo Demo


Recommended Posts

Just finished tweaking and streaming the code.

 

This code also contains a twin servo routine. The servos are connected to P1.4 and P1.5. When the X-axis moves the servo on P1.4 moves and when the Y-axis moves the servo on P1.5. The Z-aixs is implemented but not used in the program.

 

#include "msp430fr5739.h"
#include "FR_EXP.h"

#define SERVO_1 BIT4
#define SERVO_2 BIT5
#define scale 9

unsigned int servo_counter = 0;
unsigned int servo1pos = 1500;
unsigned int servo2pos = 1500;

unsigned int ADC_counter = 0;

unsigned int ADCResult_X, ADCResult_Y, ADCResult_Z;
unsigned int CalValue_X, CalValue_Y, CalValue_Z;

unsigned int temp;

void main(void)
{ 
 WDTCTL = WDTPW + WDTHOLD;  //Stop the dog
 SystemInit();  //Setup the pins        
 StartUpSequence();  //Copy Ti's fancy LED boot up trick           

 SetupAccel();  //Setup the ADC and Accel
 CalibrateADC(); //Find zero points of all 3 axis

 ADC10CTL0 |= ADC10ENC | ADC10SC; //Start the first sample. If this is not done the ADC10 interupt will not trigger.

 while (1)
 {  
   if (ADCResult_X > CalValue_X + 10)        
       {
     servo1pos = 1500 + scale * (ADCResult_X - CalValue_X);
     if (servo1pos > 2500)
     {
       servo1pos = 2500;  
     }
   }
       else if (ADCResult_X < CalValue_X - 10)
       {
         servo1pos = 1500 - scale * (CalValue_X - ADCResult_X);
         if (servo1pos < 500)
         {
           servo1pos = 500;
         }
       }
       else
       {
         servo1pos = 1500;
       }

       if (ADCResult_Y > CalValue_Y + 10)        
       {
     servo2pos = 1500 + scale * (ADCResult_Y - CalValue_Y);
     if (servo2pos > 2500)
     {
       servo2pos = 2500;  
     }
   }
       else if (ADCResult_Y < CalValue_Y - 10)
       {
         servo2pos = 1500 - scale * (CalValue_Y - ADCResult_Y);
         if (servo2pos < 500)
         {
           servo2pos = 500;
         }
       }
       else
       {
         servo2pos = 1500;
       }                                            
   }
}

void SystemInit(void) //Sets up the Fraunch board for Accel reads and LED tricks
{
 //Startup clock system in max. DCO setting ~8MHz
 // This value is closer to 10MHz on untrimmed parts  
 CSCTL0_H = 0xA5;                          // Unlock register
 CSCTL1 |= DCOFSEL0 + DCOFSEL1;            // Set max. DCO setting
 CSCTL2 = SELA_1 + SELS_3 + SELM_3;        // set ACLK = vlo; MCLK = DCO
 CSCTL3 = DIVA_0 + DIVS_0 + DIVM_0;        // set all dividers 
 CSCTL0_H = 0x01;                          // Lock Register

   // Turn off temp. 
   REFCTL0 |= REFTCOFF; 
   REFCTL0 &= ~REFON;

 // Enable LEDs
 P3OUT &= ~(BIT6+BIT7+BIT5+BIT4);
 P3DIR |= BIT6+BIT7+BIT5+BIT4;
 PJOUT &= ~(BIT0+BIT1+BIT2+BIT3);
 PJDIR |= BIT0 +BIT1+BIT2+BIT3;  

   // P3.0,P3.1 and P3.2 are accelerometer inputs
   P3OUT &= ~(BIT0 + BIT1 + BIT2);   
   P3DIR &= ~(BIT0 + BIT1 + BIT2); 
   P3REN |= BIT0 + BIT1 + BIT2;

   // Setup Servo ports
   P1OUT &= ~(SERVO_1 + SERVO_2);
   P1DIR |= SERVO_1 + SERVO_2;
   P1SEL0 &= ~(SERVO_1 + SERVO_2);
   P1SEL1 &= ~(SERVO_1 + SERVO_2);

   // Setup Servo Interrupt
   TA0CCTL0 = CCIE;
   TA0CCR0 = 1500;
   TA0CTL = TASSEL_2 + MC_1 + ID_3;
}

void CalibrateADC(void) //Sets the "zero" point of the accelerometer
{
 unsigned char CalibCounter =0;
 unsigned int Value = 0;

   __disable_interrupt();  //Turn off any interupts just incase

   ADC10CTL0 &= ~ADC10ENC; //Toggle ENC bit. Need this to change the ADC10INCH_x value.
   ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_12;  //Sample the X-axis  
 while(CalibCounter <50)
 {
   P3OUT ^= BIT4;
   CalibCounter++;
   ADC10CTL0 |= ADC10ENC | ADC10SC;  //Start sample.
   while (ADC10CTL1 & BUSY);  //Wait for sample to be done.
   Value += ADC10MEM0;
 }
 CalValue_X = Value/50;  //Average all samples to find the best zero.

 ADC10CTL0 &= ~ADC10ENC;  //Toggle ENC bit. Need this to change the ADC10INCH_x value.
 CalibCounter = 0;  //Reset the counters
 Value = 0;
 ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_13;  //Sample the Y-axis 
 while(CalibCounter <50)
 {
   P3OUT ^= BIT4;
   CalibCounter++;
   ADC10CTL0 |= ADC10ENC | ADC10SC;  //Start sample.
   while (ADC10CTL1 & BUSY);  //Wait for sample to be done.
   Value += ADC10MEM0;
 }
 CalValue_Y = Value/50;  //Average all samples to find the best zero.

 ADC10CTL0 &= ~ADC10ENC;  //Toggle ENC bit. Need this to change the ADC10INCH_x value.
 CalibCounter = 0;  //Reset the counters
 Value = 0;
 ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_14;  //Sampel the Z-axis 
 while(CalibCounter <50)
 {
   P3OUT ^= BIT4;
   CalibCounter++;
   ADC10CTL0 |= ADC10ENC | ADC10SC;  //Start sample.
   while (ADC10CTL1 & BUSY);    //Wait for sample to be done.
   Value += ADC10MEM0;
 }
 CalValue_Z = Value/50;  //Average all samples to find the best zero.

 ADC10CTL0 &= ~ADC10ENC;  //Toggle ENC bit. Need this to change the ADC10INCH_x value.
 ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_12;  //We need to start at the X-axis first due to how the interupt routine works.

 __enable_interrupt();  //enable interupts
}

void SetupAccel(void)
{  
 //Setup  accelerometer
 // ~20KHz sampling
 //Configure GPIO
 ACC_PORT_SEL0 |= ACC_X_PIN + ACC_Y_PIN + ACC_Z_PIN;    //Enable A/D channel inputs
 ACC_PORT_SEL1 |= ACC_X_PIN + ACC_Y_PIN + ACC_Z_PIN;
   ACC_PORT_DIR &= ~(ACC_X_PIN + ACC_Y_PIN + ACC_Z_PIN);
   ACC_PWR_PORT_DIR |= ACC_PWR_PIN;              //Enable ACC_POWER
   ACC_PWR_PORT_OUT |= ACC_PWR_PIN;

   // Allow the accelerometer to settle before sampling any data 
   __delay_cycles(200000);   

   //Setting up the ADC stuff 
   ADC10CTL0 &= ~ADC10ENC;                        // Ensure ENC is clear
   ADC10CTL0 = ADC10ON + ADC10SHT_5;  
   ADC10CTL1 = ADC10SHS_0 + ADC10SHP + ADC10CONSEQ_0 + ADC10SSEL_0;  
   ADC10CTL2 = ADC10RES;    
   ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_12;
   ADC10IV = 0x00;    //Clear all ADC12 channel int flags  
   ADC10IE |= ADC10IE0;  //Enable ADC10 interrupts

   __enable_interrupt();

}

void StartUpSequence(void)  //this is copied from the Ti Fraunch Pad experience. Only thing it does is make a fancy LED sequence on boot.
{
 unsigned char flag=4,up=1,counter = 0; 
 unsigned char LED_ArrayPJ[] = {0x01,0x02,0x04,0x08};
 unsigned char LED_ArrayP3[] = {0x80,0x40,0x20,0x10};

 while (counter <10) 
 {    
   counter++;
   PJOUT &= ~(BIT0 +BIT1+BIT2+BIT3);
   P3OUT &= ~(BIT4 +BIT5+BIT6+BIT7);    
   if(up)
   {
     while(flag)
     {
       P3OUT = LED_ArrayP3[flag-1];
       PJOUT = LED_ArrayPJ[flag-1];
       LongDelay();               
       flag--;
     }
     up=0;
   }
   else
   {
     while(flag<4)
     {
       P3OUT = LED_ArrayP3[flag];
       PJOUT = LED_ArrayPJ[flag];
       LongDelay();        
       flag++;
     }
     up = 1;
   }
 }   
 PJOUT &= ~(BIT0 +BIT1+BIT2+BIT3);
 P3OUT &= ~(BIT4 +BIT5+BIT6+BIT7);    
}

void LongDelay()
{
 __delay_cycles(250000);
}

//Servo Interrupt
#pragma vector = TIMER0_A0_VECTOR
__interrupt void Timer_A (void)
{  
   if(servo_counter == 0)
   {
     P1OUT |= SERVO_1;
     P1OUT |= SERVO_2;
     if (servo1pos >= servo2pos)
     {
       TA0CCR0 = servo2pos;  
     }
     else
     {
       TA0CCR0 = servo1pos;
     }
     servo_counter++;
   }
   else if(servo_counter == 1)
   {
     if (servo1pos >= servo2pos)
     {
       P1OUT &= ~(SERVO_2);
       TA0CCR0 = servo1pos - servo2pos;  
     }
     else
     {
       P1OUT &= ~(SERVO_1);
       TA0CCR0 = servo2pos - servo1pos;
     }
     servo_counter++;
   }
   else
   {
   TA0CCR0 = 20000 - servo1pos;
   P1OUT &= ~(SERVO_1);
   P1OUT &= ~(SERVO_2);
   servo_counter = 0;  
   }
}

//ADC10 interupt routine
#pragma vector = ADC10_VECTOR
__interrupt void ADC10_ISR(void)
{   
 if (ADC_counter == 0) //X-axis
 {
   ADC10CTL0 &= ~ADC10ENC;  
   ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_13;  //Next channel is the Y-axis
   ADCResult_X = ADC10MEM0;
   ADC_counter++;
   ADC10CTL0 |= ADC10ENC | ADC10SC;
 }
 else if (ADC_counter == 1)  //Y-axis
 {
   ADC10CTL0 &= ~ADC10ENC;
   ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_14;  //Next channel is the Z-axis
   ADCResult_Y = ADC10MEM0;
   ADC_counter++;
   ADC10CTL0 |= ADC10ENC | ADC10SC;
 }
 else  //Z-axis
 {
   ADC10CTL0 &= ~ADC10ENC;
   ADC10MCTL0 = ADC10SREF_0 + ADC10INCH_12;  //Next channel is the X-axis
   ADCResult_Z = ADC10MEM0;
   ADC_counter = 0;
   ADC10CTL0 |= ADC10ENC | ADC10SC;
 }
}

 

http://longhornengineer.com/projects/code/msp-exp430-accel-and-servo/

Link to post
Share on other sites
  • 1 month later...
  • 1 year later...

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

×
×
  • Create New...