' PICBASIC PRO program to read IMU 5 d.o.f. for Balancing Robot ' Use AD converters to read 3-Axis Accelerometer and 2-Axis Gyros ' Use 3.3 volt power supply as vRef for AD coversions. ' Uses Timer1 to accurately sample the gyro at 50Hz ' Kalman_Gyro_Update is updating the gyro angle correctly, ' Kalman_Angle_Update seems to work... sort of. ' The bias value gets updated slowly, but only to a point. ' Had to change the sign of the Accel angle to match the gyro. ' I'd like to calibrate, or verify the sensors against a protractor. ' v6 - Kalman stuff removed. It was bunk. No effect. ' Replaced with self-modifying gyro bias routine. ' Works good. Gyro and Tilt sensor working together. ' v7 - Added Interupt routines. Working well. ' Interupts now trigger 50Hz Gyro Heartbeat by setting flag every 20ms ' Timer 2 goes off every 10ms. Every other cycle triggers Gyro Flag ' Timer 1 will be used to time Servo Pulse widths ' v8 Moved to Balance Bot Board - use 16F873A @ Device HS_OSC ' High Speed Crystal @ Device WDT_ON ' Watch Dog timer ON @ Device PWRT_ON ' Power Reset timer ON @ Device BOD_ON ' Brown out Protection ON @ Device LVP_OFF ' Low voltage programming capability @ Device CPD_OFF ' Code Protection OFF define __16F876A 1 define OSC 20 OSC_SPEED con 20 'define PicBasic constant DEFINE INTHAND DO_ISR ' Shiftin Modes Symbol MSBPRE = 0 ' MSB first, sample before clock Symbol LSBPRE = 1 ' LSB first, sample before clock ' Shiftout Modes Symbol LSBFIRST = 0 ' LSB first Symbol MSBFIRST = 1 ' MSB first '-------------- debug definitions ----------------------------- DEFINE DEBUG_REG PortC DEFINE DEBUG_BIT 6 DEFINE DEBUG_BAUD 19200 DEFINE DEBUG_MODE 0 ' ------------- Hardware Addresses --------------------------- GIE VAR INTCON.7 ' Global Interupt Enable PEIE VAR INTCON.6 ' Perif. Eq. Interupt Enable ADIE VAR PIE1.6 ' A-2-D Converter Interupt Enable TMR1IE VAR PIE1.0 ' PIE address 10h TMR2IE VAR PIE1.1 TMR1ON VAR T1CON.0 ' Register addresses are in PIC.inc TMR2ON VAR T2CON.2 ' ------------------------------------------------------------------ LED var PORTC.3 '-------------- uM-FPU pin definitions ------------------------ FpuClk var PORTC.0 FpuIn var PORTC.1 FpuOut var PORTC.2 '-------------- uM-FPU register definitions ------------------ angle con 1 ' current Angle estimate rate con 2 ' current Gyro rate q_bias con 3 ' Gyro Bias dT con 4 ' dT = 50Hz = 0.02 Err con 5 ' Registers used in Angle_Update Angle_Err con 6 ' uM-FPU register tmpAngle CON 7 ' used in Calc_xAngle xAngle CON 8 ' angle via tilt sensor Kp CON 9 ' Scalers for PID equation Kd CON 10 Ki CON 11 nAngle CON 12 ' Angle of neutral balance bTorque CON 13 ' Balance Torque prevAngle CON 14 ' for derivitive value tmp CON 15 ' uM-FPU register 15 ' ------------- Allocate space for Interupt Storage ----------------------- wsave VAR BYTE $20 SYSTEM ; Save W reg location if in bank0 wsave1 VAR BYTE $A0 SYSTEM ; Save W reg location if in bank1 wsave2 VAR BYTE $120 SYSTEM ; Save W reg location if in bank2 wsave3 VAR BYTE $1A0 SYSTEM ; Save W reg location if in bank3 ssave VAR BYTE Bank0 SYSTEM ; Save location for STATUS reg psave VAR BYTE Bank0 SYSTEM ; Save location for PCLATH reg rPulseWidth VAR WORD Bank0 lPulseWidth VAR WORD Bank0 StatFlag VAR BYTE Bank0 GyroTaskFlag VAR StatFlag.0 OddCycleFlag VAR StatFlag.1 '------------- Allocate variables ---------------------------- adval VAR WORD nLoops VAR WORD X_Val VAR WORD GyroValue VAR WORD ' raw gyro measurement GyroBias Var Word myAngle VAR WORD Torque VAR WORD Torque2 VAR WORD tmpWord VAR Word tmpByte VAR Byte n var byte ' ------------------------------------------------------------------ include "umfpuV2-spi.inc" ' include the uM-FPU support routines ' ------------------------------------------------------------------ Initialize: Pause 400 ' ADCON1 = %10000000 ' Set AD clock to Foxc/64, AD uses +5v as +VRef ADCON1 = %10000011 ' Set AD clock to Foxc/64, AD uses +3.3v as +VRef TRISA = %00101111 ' Set PORTA Input/Output : 1=Input : 0=Output TRISB = %00000000 TRISC = %10000100 ' Setup PORTC ' T1CON = %00110000 ; Timer1 1:8 pre-scaler - used for 20ms timer T1CON = %00000000 ; Timer1 1:1 pre-scaller - for 1.5ms pulse timer T2CON = %01111011 ; Timer2 Pre- and Post-Scalers = 1:16, Timer OFF TMR2 = 0 ; This also clears pre and post scalers PR2 = 195 ; Set Timer2 Match Register for 10 ms. @ 20MHz ' TMR1ON = 1 ; Turn Timer1 ON for 20ms basic timer TMR1IE = 1 ; Timer1 Interupt Enable bit for servo pulses StatFlag = 0 rPulseWidth = 65535 - (5 * 1500) ' neutral position = 1.5ms pulse lPulseWidth = rPulseWidth Pause 200 DEBUG 13,10,"IMU 5-DOF Test v9", 13 HIGH LED '------------------------------------------------------------ ' Reset the uM-FPU Floating Point processor GOSUB Fpu_Reset 'reset the FPU hardware IF fpu_status <> SyncChar THEN Debug "uM-FPU not detected.",13 END ENDIF ' Load FPU with inital values ' dT = 0.02 for 50Hz gyro integration SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ Kp, ATOF, "36.0", 0, FSET,_ Kd, ATOF, "0.0", 0, FSET,_ Ki, ATOF, "0.0", 0, FSET] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [dT, ATOF, "0.02", 0, FSET] GOSUB Fpu_Wait myAngle = 86 GyroBias = 464 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ Angle, LOADWORD, myAngle\16, FSET,_ xAngle, FSET+angle,_ prevAngle, FSET+angle,_ q_bias, LOADWORD, GyroBias\16, FSET] ' Setup initial angle of neutral balance myAngle = 86 ' myAngle = 90 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ nAngle, LOADWORD, myAngle\16, FSET,_ bTorque, XOP, LOADZERO, FSET] GOSUB Fpu_Wait2 '============================================================================ '============================================================================ Goto mainloop ' Skip over subroutines '============================================================================ '============================================================================ TurnON_Timer2: GIE = 1 ' Global Interupt Enable PEIE = 1 ' Peripheral Interupt Enable ' Setup Timer2 to trigger interupts every 10 ms ' This starts chain of repeating events TMR2IE = 1 ; Timer2 Interupt Enable bit TMR2ON = 1 ; Turn ON timer2 RETURN '============================================================================ ' Do_ISR ' Interupt Service Routine ' Handle all the Interups Asm DO_ISR IF (CODE_SIZE <= 2) movwf wsave ; save W register swapf STATUS,W ; save STATUS reg clrf STATUS ; switch to bank 0 movwf ssave ; save STATUS reg to area in Bank 0 movf PCLATH,W ; save Program Counter movwf psave ; save in Bank 0 location EndIF ; ========================================================================== ; See who caused the Interupt. ; Vector to the correct routines BTFSC PIR1,TMR1IF ; Timer1 overflow? GOTO T1_INT ; Yes - go to Timer1 routine BTFSC PIR1,TMR2IF ; Timer2 overflow? GOTO T2_INT ; Yes - go to Timer2 routine CLRF PIR1 GOTO ISR_Done ; Error - never handled interupt ; =============================== T1_INT ; Handle Timer1 Overflow - signals end of Pulse BCF T1CON,TMR1ON ; Turn OFF timer1 BCF PIR1, TMR1IF ; Clear the interupt flag BCF PORTB,5 ; Turn Off Pin Pulse - turn both pins off BCF PORTB,4 ; only 1 will be on, but clearing both is OK GOTO ISR_Done ; ============================================================================ T2_INT ; Handle Timer2 Overflow interupt - every 10ms BCF T2CON,TMR2ON ; Turn OFF Timer2 BCF PIR1, TMR2IF ; Clear the interupt flag CLRF TMR2 ; Timer2 setup for another 10ms interupt BSF T2CON,TMR2ON ; Turn ON Timer2 BTFSS _StatFlag,1 ; Odd or Even Cycle? GoTo DoEven Do_ODD ; Generate 1.5ms servo pulse BCF _StatFlag,1 ; Next Cycle is EVEN ; GoTo ISR_Done ; Do nothing for now ; Setup to send out Pulse - Turn off via Timer1 interupt ; fill Timer1 with 65535 - (5*TimerTicks) MOVF _rPulseWidth,w MOVWF TMR1L MOVF _rPulseWidth+1,w MOVWF TMR1H ; BSF PIR1,TMR1IE ; Timer1 Interupt Enable BSF T1CON,TMR1ON ; Turn ON timer1 BSF PORTB,5 ; Start 1.5ms Right Servo Pulse GoTo ISR_Done ; Generate another interupt in 10ms ; ------------------------------ DoEven ; Set flag to trigger Gyro reading BSF _StatFlag,1 ; Next Cycle is ODD BSF _StatFlag,0 ; Trigger 50Hz 20ms Heartbeat ; GOTO ISR_Done ; Generate another interupt in 10ms ; do left servo pulse here... MOVF _lPulseWidth,w MOVWF TMR1L MOVF _lPulseWidth+1,w MOVWF TMR1H ; BSF PIR1,TMR1IE ; Timer1 Interupt Enable BSF T1CON,TMR1ON ; Turn ON timer1 BSF PORTB,4 ; Start 1.5ms Pulse On Left Servo Pin ; GoTo ISR_Done ; Generate another interupt in 10ms ; =============================== ISR_Done Movf psave,w ; Restore the PCLATH reg Movwf PCLATH swapf ssave,w ; Restore the STATUS reg ;(sets bank to original state) Movwf STATUS ; move W into Status reg swapf wsave,f ; Swap w_save swapf wsave,w ; Swap w_save into W RETFIE EndASM '============================================================================ '============================================================================ ' Integrate_Gyro is called every dT time period (50Hz) ' The Angle value within the FPU is updated based on the current rate. ' Enter with GyroValue containing raw data from sensor. ' Bias is removed within the FPU ' Gyro returns 500 degrees per second at full scale. ' Our 10-Bit full scale value is 512 - close enought for 1:1 scaling. Integrate_Gyro: ' Update gyro rate and angle values ' rate = q_m - q_bias ' angle += rate * dT ' save prev angle to calc derivitive value SHIFTOUT FpuOut, FpuClk, MSBFIRST,[_ prevAngle, FSET+angle,_ rate, LOADWORD, GyroValue\16, FSET, FSUB+q_bias,_ SELECTA, FSET+rate, FMUL+dT, angle, FADD] RETURN '---------------------------------------------------------------------------- ' Balance_Torque = Kp * (Current_Angle - Balance_angle) + ' Kd * (derivitive value) Calc_Balance_Values: SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ tmp, FSET+angle, FSUB+prevAngle,_ tmpAngle, FSET+angle, FSUB+nAngle, FMUL+Kp,_ bTorque, FSET+Kd, FMUL+tmp, FADD+tmpAngle] ' SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ ' bTorque, FSET+angle, FSUB+nAngle, FMUL+Kp] GOSUB Fpu_Wait2 ' bTorque now holds complete PD value RETURN ' ------------------------------------------------------------------------- ' Use the formula PulseONTime = 65535 - (5 * 1500) with 1:1 pre-scaler ' Use Pulse_ON_Time = 65535 - ((Position * 5) / 2) with 1:2 pre-scaler ' If only timing ON pulse widths - use 1:1 scaler. ' If I have to time both PulseON and PulseOFF times, use 1:2 scaler Setup_PulseWidth: Gosub Read_Torque ' Torque is Kp * (Angle-nAngle) = [0..400] ' IF Torque = -14081 Then ' Debug 13,13," Re-read Torque...",13 ' Gosub Read_Torque ' Torque = Torque2 ' EndIf IF (abs Torque) > 1600 Then ' some kind of noise error Debug 13, " Torque Error ", sdec torque, 13 Gosub Print_FPU Torque = Torque2 EndIf Torque2 = Torque ' keep copy of previous value tmpByte = Torque.15 ' sign bit tmpWord = ABS Torque ' for now... tmpWord = tmpWord MIN 500 if tmpByte then tmpWord = tmpWord * (-1) Endif ; Check for Abs(current_angle) < 45 degrees tilt away from balance point ; otherwise robot has fallen over IF (myAngle < 50) OR (myAngle > 130) then rPulseWidth = 65535 - (5 * 1500) ' 1500 = 1.5ms Pulse Width lPulseWidth = rPulseWidth Else rPulseWidth = 65535 - (5 * (1500-tmpWord)) lPulseWidth = 65535 - (5 * (1500+tmpWord)) EndIf RETURN '-------------------------------------------------------------------- ' Calculate the angle of inclination from the X-Axis Accelerometer ' Enter with X_Val as raw measurement from sensor. ' Store xAngle value in FPU register. Calc_xAngle: ' xAngle = 90 - ((X_Val - 512) / 2) tmpWord = 512 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [tmp, LOADWORD, tmpWord\16, FSET] ' tmpAngle = (X_Val - 512) / 2 tmpByte = 2 SHIFTOUT FpuOut, FpuClk, MSBFIRST,_ [tmpAngle, LOADWORD, X_Val\16, FSET, FSUB+tmp, LOADBYTE, tmpByte, FDIV] ' xAngle = 90 - (X_Val-512) / 2 tmpWord = 90 SHIFTOUT FpuOut, FpuClk, MSBFIRST,_ [xAngle, LOADWORD, tmpWord\16, FSET, FSUB+tmpAngle] Gosub Fpu_Wait2 RETURN '-------------------------------------------------------------------- ' Called when we have a new measurement from the Accelerometer ' Use xAngle .. the current angle as measured by the X-Axis accel. ' Use that value to calc the error with our estimate. xAngle_Update: ' Angle_Err = xAngle - angle SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Angle_Err, FSET+xAngle, FSUB+angle] '----------------------- ' Update angle and bias registers ' angle += (0.2 * angle_err); ' q_bias += (0.08 * angle_err); SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Err, ATOF, "0.2", 0, FSET,_ tmp, FSET+Angle_Err, FMUL+Err, angle, FADD+tmp] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Err, ATOF, "0.08", 0, FSET,_ tmp, FSET+Angle_Err, NEGATE, FMUL+Err, q_bias, FADD+tmp] GOSUB Fpu_Wait ' ... now the new angle and bias values are available RETURN ' -------------------------------------------------------------------------- Print_Angle_Err: Debug " FPU Angle: " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Angle] Gosub Print_Float Debug " Angle Err = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Angle_Err] Gosub Print_Float Debug 13 RETURN Print_xAngle: ' FIX puts Int value of A-reg into register zero SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+tmp, FSET+xAngle, ROUND, FIX,_ SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [myAngle.HIGHBYTE, myAngle.LOWBYTE] Debug " xAngle = ", SDEC myAngle,13 RETURN Print_FPU: Debug 13, "----- FPU -----", 13 Debug " bTorque = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [bTorque] Gosub Print_Float Debug " Rate = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [rate] Gosub Print_Float Debug 13, " Angle = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Angle] Gosub Print_Float Debug " xAngle = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [xAngle] Gosub Print_Float Debug " AngleErr = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Angle_err] Gosub Print_Float Debug 13,13 RETURN Print_Torque: Debug " bTorque = " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [bTorque] Gosub Print_Float RETURN Print_Gain: Debug " Kd: " SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Kd] Gosub Print_Float Debug 13 RETURN Read_Angle: ' FIX puts Int value of A-reg into register zero SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+tmp, FSET+angle, ROUND, FIX] Gosub FPU_Wait2 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus 100 shiftin FpuIn, FpuClk, MSBPRE, [myAngle.HIGHBYTE, myAngle.LOWBYTE] RETURN Read_Bias: ' FIX puts Int value of A-reg into register zero SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+tmp, FSET+q_bias, ROUND, FIX,_ SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [GyroBias.HIGHBYTE, GyroBias.LOWBYTE] RETURN Read_Torque: SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+tmp, FSET+bTorque, ROUND, FIX] Gosub FPU_Wait2 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus 100 shiftin FpuIn, FpuClk, MSBPRE, [Torque.HIGHBYTE, Torque.LOWBYTE] RETURN '============================================================================ '============================================================================ ' Subroutines to read AD converter voltages Read_AD: nLoops = 0 Pauseus 10 ' Wait for channel to setup ADCON0.2 = 1 ' Start conversion wLoop: nLoops = nLoops + 1 Pauseus 5 ' Wait for conversion IF ADCON0.2 = 1 then wLoop adval.HIGHBYTE = ADRESH adval.LowBYTE = ADRESL Return '----------------------------------------------------- ' Subroutines to read IMU 5 dof sensor voltages Read_Tilt_Sensor: ADCON0 = %10100001 ' Set A/D to Channel 4, On Goto Read_AD Read_Gyro: ADCON0 = %10010001 ' Set A/D to Channel 2, On Goto Read_AD Read_Pot2: ADCON0 = %10001001 ' Set A/D to Channel 1, On Goto Read_AD Read_Pot1: ADCON0 = %10000001 ' Set A/D to Channel 0, On Goto Read_AD '---------------------------------------------------------------------------- ' Setup Kp value by reading potentiometer / 10 = [0.0 ... 102.3] Calc_Gain: Gosub Read_Pot1 tmpWord = adval tmpByte = 10 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ tmp, LOADBYTE, tmpByte, FSET,_ Kp, LOADWORD, tmpWord\16, FSET, FDIV+tmp] Gosub Read_Pot2 tmpWord = 512 - adval tmpByte = 10 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ tmp, LOADBYTE, tmpByte, FSET,_ Kd, LOADWORD, tmpWord\16, FSET, FDIV+tmp] RETURN '============================================================================ '============================================================================ mainloop: debug "Made it to the Main Loop v9", 13,13 GOSUB Calc_Gain ' Read Pot - setup Kp in FPU Gosub Read_Tilt_Sensor X_Val = adval Gosub Calc_xAngle Gosub xAngle_Update Gosub TurnON_Timer2 ' Start the 50Hz Heartbeat Pause 500 ' ------- gLoop: GOSUB Calc_Gain ' Gosub Print_Gain ' Gosub Read_Gyro ' GyroValue = adval ' Debug " Gyro: ", Dec GyroValue, " rate: ", sdec GyroValue-464 ' Gosub Read_Bias ' Debug " Bias: ", dec GyroBias Gosub Read_Angle ' this is needed to detect when bot has fallen over ' Debug " gAngle: ", dec myAngle ,13 ' Debug " Torq: ", SDEC Torque ' Gosub Print_Angle_Err ' Gosub Print_Torque ' Gosub Print_xAngle Toggle LED For n = 1 to 50 ' This seq runs 50 times per second Gosub Read_Gyro : GyroValue = adval Gosub Integrate_Gyro Gosub Calc_Balance_Values Gosub Setup_PulseWidth IF (n // 10) = 0 Then ' Read the Accelerometer @ 5 Hz - Calc xAngle Gosub Read_Tilt_Sensor X_Val = adval Gosub Calc_xAngle Gosub xAngle_Update EndIf waitLoop: IF (StatFlag.0 == 0) then waitLoop StatFlag.0 = 0 ' clear the flag - asm will set it after 20ms Next n Goto gLoop debug 13, "All Done.", 13 Pause 1000 LOW LED END