//what are we trying to do here // we would like to generate a 1KHz PWM //also generate an interrupt at about 20Hz. // //how to do this? //use tmer B for the PWM signal, running off of the DCO. //use Timer A as the interrupt source, running of the ACLK. // #include #define abs(a) (((a)>=0)?(a):(-a)) //unsigned int pwm_value = 0; //where we are long int position; //inermediate position steps along the way long int intermediate; //how difference between intermediate and current position long int error; //how far to move long int relative; //what is the accel/deaccel rate long int accel; //desired final position long int goal_position; // current distance from goal position long int dist_remain; // dist to stop if motor began to decelerate long int dist_stop; //some of the PID parameters long int Kp; long int Ki; long int Kd; //how large the integrator can get long int int_limit; //fastest it can move long int v_limit; //PID us the loop output long int PID; //some other state variables char stop_flag; //char direction; char tempflag; //hold last position from decoder long int last_position = 0; //hold the current velocity long int current_velocity; //hold the ingeration sum long int sum = 0; //hold the last error value for derivative long int last_error = 0; //hold the current commanded velocity, rate to change the command position long int cmd_vel = 0; //this routine is called from within the timer interrupt //it takes in a number and send its ASCII equiv to the serial port void charsender ( unsigned char x) { unsigned char tmp; tmp = (x & 0xf0)>> 4; //strip off lower nibble and shift if (tmp < 0x0a) { while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = tmp + 0x30; // convert to ascii and send } else { while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = tmp + 55; // convert to ascii and send } tmp = x & 0x0f; //strip off upper nibble if (tmp < 0x0a) { while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = tmp + 0x30; // convert to ascii and send } else { while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = tmp + 55; // convert to ascii and send } } void startmove(void) { //find final location goal_position = position + relative; stop_flag=0; } //this is the PID servo loop void servoloop(void) { //calculate distance remaining dist_remain=goal_position-last_position; //calculate the current velocity current_velocity = intermediate-last_position; //calculate how long to stop dist_stop=0.5*current_velocity*current_velocity/accel; //and save for next time last_position = intermediate; //are we still moving in the same direction? if((cmd_vel*dist_remain)>=0) { // is it time to begin stopping? if((abs(dist_remain)>abs(dist_stop))&(stop_flag==0)) { //keep going, not time to stop if(dist_remain>0) { cmd_vel+=accel; if (cmd_vel > v_limit)cmd_vel = v_limit; } else { cmd_vel-=accel; if (-cmd_vel > v_limit)cmd_vel = -v_limit; } } else { //time to decel stop_flag=1; if (cmd_vel >0) cmd_vel -= accel; else cmd_vel+=accel; } } //just changed direction need to decelerate to v=0 else { if(cmd_vel>0) cmd_vel-=accel; else cmd_vel+=accel; } if((abs(dist_remain)<5)&(abs(cmd_vel) int_limit) sum = int_limit; else if ( sum < -int_limit) sum = -int_limit; //calc the integral portion // PID += sum / Ki; //now do the derivative // PID += Kd * (error - last_error); //update errors last_error = error; //now output the result //if negative then negate result but set dir if (PID < 0 ) { P4OUT |= 0x04; //set for reverse PID = PID * -1; //flip the PID value to positive } else P4OUT &= 0xfB; //set for forward // encoders can't handle much faster than 700 for old demo board if (PID > 500) PID = 500; //must leave some off time for the //the driver to recharge //maintain small dead band //if((stop_flag==1)&(PID<5)) PID=0; //update the PWM TBCCR1 = PID; //P1.0 LED control if (error == 0) P1OUT |= 0x01; //turn LED on else P1OUT &= 0xFE; // turn LED off; } //this is the timer a up to ccr0 interrupt //just toggle the led and reti //need to read the TAIV register to clear the //interrupt request flag, and branch to the correct routine. interrupt [TIMERA1_VECTOR] void timeraov(void) { unsigned char *pos_bytes; //SET TIMMING SIGNAL HIGH P3OUT |= 0X01; pos_bytes = (unsigned char *)&position; switch (TAIV) //read TAIV to determine reason for interrupt { //case 10 would be the timer ov. case 10: ADC12CTL0 |= ADC12SC; // Start conversion // P1OUT ^= 0x01; //update the LED // while ((ADC12IFG & ADC12BUSY)==0); //wait for converstion to finish // TBCCR1 = (ADC12MEM0 >> 2); //transfer to PWM break; default: break; } //now read the encoder counter //read address 000 to grab encoder 0's count P2OUT=0x10; while ((P2IN & 0x20)==0); // wait for readly line to do high (ready is P2.5) pos_bytes[0] = P5IN; //ready 1st byte and put into position1 P2OUT=0x14; pos_bytes[1] = P5IN; P2OUT=0x16; pos_bytes[2] = P5IN; P2OUT=0x18; pos_bytes[3] = P5IN; P2OUT &=0x01; //de-select charsender(pos_bytes[3]); //read data and send out ascii; charsender(pos_bytes[2]); //read data and send out ascii; charsender(pos_bytes[1]); //read data and send out ascii; charsender(pos_bytes[0]); //read data and send out ascii; while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = 0x0d; //carrige return while ((IFG1 & UTXIFG0) == 0); // USART0 TX buffer ready? TXBUF0 = 0x0a; // linefeed while ((P2IN & 0x20)==0x20); // wait for readly line to do low //now do the servo thing servoloop(); //AND NOW END TIMING SIGNAL P3OUT &= 0XFE; } //main routine void main(void) { WDTCTL = WDTPW + WDTHOLD; // Stop watchdog timer //first set up the DCO for approximatly 1MHz //these defines can be found in the msp430x140x.h file BCSCTL1 |= RSEL0; //Need to define port 4.1 as output and use second fucntion //and port 4.2 as direction output //P4DIR |= 0x06; P4SEL |= 0x02; // why is this in peripheral mode? P4OUT = 0x00; // clear P4 output register P4DIR = 0xff; // unused pins as outp P4OUT|=0x01; // clear encoder counter by homing //make sure LED is output, P1.0 is LED output P1DIR |= 0x01; P1OUT &=0xfe; //setup port 2.0 in peripheral mode for 32KHz out P2SEL |= 0x01; //configure port 2 for encoder decoder control //P2.0=CLK, P2.1=A0, P2.2=A1, P2.3=A2, P2.4=SELECT, P2.5=READY, P2.6=Unassigned, P2.7=INTR //define port 2.0-2.4, and 2.6 as ouput //P2DIR |=0x1f; P2DIR =0x9f; P2OUT &=0x01; //Set P2.1-7 to all 0 //Now set timer B for PWM mode using SMCLK //first stop and clear the counter AND USE smclk TBCTL = (TBSSEL1 | TBCLR); //Then set what to count up to. TBCCR0 = 0x3E8; //Which makes timer B divided by 1000 //Then set up compare fucntions TBCCTL1 = 0x0240; //Then the width of the pulse SET TO 0 TBCCR1 = 0; //Now startup Timer B IN UP/DOWN MODE TBCTL |= (MC1 | MC0); P6SEL |= 0x01; // Enable A/D channel A0 ADC12CTL0 = ADC12ON+SHT0_2; // Turn on ADC12, set sampling time ADC12CTL1 = SHP; // Use sampling timer ADC12CTL0 |= ENC; // Enable conversions //Now that the PWM is running let's setup the interrupt! //first stop timer A TACTL = (TACLR | TASSEL0); //Then set what to cont to 32Hz TACCR0 = 1024; //now start timer a with interrupts on, and count to CCR0. TACTL |= (TAIE | MC0); //now configure the serial port UCTL0 = CHAR; // 8-bit character UTCTL0 = SSEL0; // UCLK = ACLK //this is set for a 9600 baud rate UBR00 = 0x03; // 32k/2400 - 13.65 UBR10 = 0x00; // UMCTL0 = 0x4A; // modulation ME1 |= UTXE0 + URXE0; // Enable USART0 TXD/RXD P3SEL |= 0x30; // P3.4,5 = USART0 TXD/RXD P3DIR |= 0x10; // P3.4 output direction //use port 3.0 as a timing signal P3DIR |= 0x01; P3OUT &= 0xFE; //port 5 is data bus; P5DIR=0x00; P5OUT=0x00; P1OUT &= 0xFE; //turn LED off tempflag=1; //setup for move //set up some values stop_flag=0; Kp = 2; Ki = 1000; Kd = 0; accel = 2; v_limit = 33; int_limit = Ki * 50; PID = 0; //where we are position = 0; //how far to move relative = 1000; //zero home switch P4OUT&=0xFE; //command postion to where we are intermediate = last_position = position; //setup to do move startmove(); servoloop(); //start global interrupts _EINT(); while (1) { //at location if((position>=5000)&(tempflag==1)) { goal_position = 4000; stop_flag=0; tempflag=0; } /* if (intermediate == goal_position) { while (error != 0); relative = 0; startmove(); }; */ }; }