I made a simple pid speed control some time ago, not very scientific but it works o.k.
I used a surplus motor with an optical encoder, then used the 'count' command to measure speed. I looked at the parallax code and copied their way of dealing with negative numbers.
here's my code:
Code:<font color="#008000">@ DEVICE PIC16F88, MCLR_OFF, INTRC_OSC_NOCLKOUT, WDT_ON, LVP_OFF, BOD_OFF, PWRT_ON, PROTECT_OFF, CCPMX_ON </font><b>DEFINE </b>OSC 8 <b>DEFINE </b>CCP1_REG PORTB <b>DEFINE </b>CCP1_BIT 3 <font color="#000080"><i>' portb.3 motor PWM output </i></font><b>DEFINE </b>ADC_BITS 8 <font color="#000080"><i>' Set number of bits in result </i></font><b>DEFINE </b>ADC_CLOCK 3 <font color="#000080"><i>' Set clock source (3 = RC) </i></font><b>DEFINE </b>ADC_SAMPLEUS 50 <font color="#000080"><i>' Set sampling time in uS </i></font>PORTA = %00000000 TRISA = %00000001 PORTB = %00000000 TRISB = %00010000 OSCCON = %01111000 <font color="#000080"><i>'8MHZ clock </i></font>ANSEL = %00000001 CMCON = %00000111 led <b>VAR </b>PORTA.1 sensor <b>VAR </b>PORTB.4 <font color="#000080"><i>'optical feedback from motor </i></font>counter <b>VAR BYTE </b>set_speed <b>VAR WORD </b>actual_speed <b>VAR WORD</b> error <b>VAR WORD </b>old_error <b>VAR WORD </b>error_sum <b>VAR WORD </b>sign <b>VAR WORD </b>Kp <b>CON </b>1 Ki <b>CON </b>2 <font color="#000080"><i>'inverse !! </i></font>Kd <b>CON </b>0 power <b>VAR WORD </b>P <b>VAR WORD </b>I <b>VAR WORD </b>D <b>VAR WORD CLEAR </b><font color="#000080"><i>'clear all variables </i></font>loop: <b>GOSUB </b>read_sensors <b>GOSUB </b>calculate_error <b>GOSUB </b>calculate_proportional <b>GOSUB </b>calculate_integral <b>GOSUB </b>calculate_differential <b>GOSUB </b>calculate_drive <b>HPWM </b>1,power,10000 <font color="#000080"><i>' drive motor </i></font><b>TOGGLE </b>led <b>GOTO </b>loop read_sensors: <b>ADCIN </b>0,set_speed <font color="#000080"><i>'read potmeter </i></font><b>COUNT </b>sensor,20,actual_speed <font color="#000080"><i>'count number of pulses </i></font><b>RETURN </b><font color="#000080"><i>'from encoder in 20msec </i></font>calculate_error: error = set_speed - actual_speed <b>RETURN </b>calculate_proportional: P = Kp * error <b>RETURN </b>calculate_integral: error_sum = error_sum + error sign = error_sum <b>GOSUB </b>SetSign <b>IF ABS </b>error_sum > (250*Ki) <b>THEN </b>error_sum = sign * (250*Ki) <font color="#000080"><i>' limit </i></font>I = <b>ABS </b>error_sum / Ki I = sign * I <b>RETURN </b>calculate_differential D = Kd * (error - old_error) old_error = error <b>RETURN </b>calculate_drive: power = P + I + D sign = power <b>GOSUB </b>SetSign <b>IF </b>sign = 1 <b>THEN </b><font color="#000080"><i>' no negative power </i></font><b>IF </b>power > 255 <b>THEN </b>power = 255 <b>ELSE </b>power = 0 <b>ENDIF RETURN </b>SetSign: <b>IF </b>sign.Bit15 = 0 <b>THEN </b>sign = 1 <b>ELSE </b>sign = -1 <b>ENDIF RETURN END </b>




Bookmarks