'********************************************************************* '* Name : LocoMotion_v12.pbp '* Author : Pete Burnight '* Notice : Copyright (c) 2007 Central Coast Software & Swarm Orbs '* : To be released under GNU Public License '* Date : 2/27/07 '* Version : 10.1 '* Notes : Testing first PIC carrier board for 16F877A '* : Working on Quad Decoders... they now work. '* : v3 works; ie: doesn't crash. '* : v5 added Servo interupts & they work great. '* '* - v6 Adding TickTimer clock. Adding Steering routines. '* TickTimer is running too fast (x10) 250 = 2.50 seconds '* Calc Angles works great - returns Steering Axis and Swing Angle '* '* - v7 Changed Steering & Speed Ranges to [-100.0 <- 0 -> 100.0] '* - v8 Added I2c Slave routines. Works with Orb Xmitter !!! '* - v9 Adding Data Logger & constant Velocity routines... '* - v10 - Working on constant velocity routines - they run-away. '* - v11 - Changing the Velocity PID functions. '* - v12 - Finishing the Gyro Tracking Functions. '* '*********************************************************************** @ Device HS_OSC ' High Speed Crystal @ Device WDT_ON ' Watch Dog timer ON @ Device PWRT_ON ' Power Reset timer ON @ Device BOD_OFF ' Brown out Protection OFF @ Device LVP_OFF ' Low voltage programming capability @ Device CPD_OFF ' Code Protection OFF define __16F877A 1 define OSC 20 OSC_SPEED CON 20 DEFINE INTHAND DO_ISR DEFINE HSER_BAUD 9600 'DEFINE HSER_RCSTA 90h DEFINE HSER_TXSTA 24h DEFINE HSER_SPBRG 129 '--------------------- Define registers and bits -------------------------- DEFINE I2C_BUFFERLEN 12 DEFINE I2C_TXBUFLEN 16 DEFINE I2C_DBLBUFLEN 8 ' 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 ' --- Command Constants ----------------- SET_SPEED CON $01 SET_STEER CON $02 SET_VELOCITY CON $03 SET_STEER_ANGLE CON $04 SET_VALUE CON $30 SET_VALUE1 CON $31 SET_VALUE2 CON $32 LOG_ON_OFF CON $2A HEART_BEAT CON $8A ' ------------- 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 ' Timer1 Interupt Enable - Servo Pulses TMR2IE VAR PIE1.1 ' Timer2 - Quad Wheel Encoders SSPIE VAR PIE1.3 ' I2c - SSP Interupt Enable TMR1ON VAR T1CON.0 ' Timer 1&2 ON/OFF switches TMR2ON VAR T2CON.2 '-------------- uM-FPU register definitions ------------------ fpuSwingAngle CON 1 ' Angles tracked by Gyros fpuSteerAngle CON 2 SwingTilt CON 3 ' angle via accelerometers SteerTilt CON 4 x_bias con 5 ' Gyro Bias y_bias con 6 ' Gyro Bias rate con 7 ' current Gyro rate dT con 8 ' dT = 50Hz = 0.02 Err con 9 ' Registers used in ?? Angle_Err con 10 ' uM-FPU register tmpAngle CON 11 ' used in Calc_Bias TiltAngle CON 12 ' angle via tilt sensor xVal CON 13 yVal CON 14 tmp CON 15 ' uM-FPU register 15 '-------------- uM-FPU pin definitions ------------------------ FPUclk var PORTB.5 FPUin var PORTB.4 FPUout var PORTB.4 '------- I/O Pins ------------------------------------------- LED VAR PORTD.2 ' LED signals Heart Beat EnaPin VAR PORTC.2 ' PWM pin to H-Bridge FwdPin VAR PORTC.1 RevPin VAR PORTC.0 QuadPin1 Var PORTB.1 QuadPin2 Var PORTB.2 ServoPin Var PortD.3 ' ------------- 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 fSave VAR BYTE Bank0 SYSTEM ' --------------------------------------------------------------------------- ' Allocate variables ' --------------------------------------------------------------------------- T1_10ms_Value Var WORD Bank0 Pulse_ON_Time Var WORD Bank0 Pulse_OFF_Time Var WORD Bank0 TickTimer Var Word Bank0 SteeringTimer VAR Byte Bank0 QuadCount VAR Byte Bank0 ; Count [-127..0..127] encoder clicks QuadPins Var Byte Bank0 ; current state of pins QuadPrev VAR Byte Bank0 ; previous state of Quad Encoder pins QuadByte Var Byte Bank0 QuadError Var Byte Bank0 QStack Var Byte[6] ServoFlags VAR BYTE Bank0 Wait20msFlag Var ServoFlags.0 Wait10msFlag VAR ServoFlags.1 ' --- I2c_BUFLEN CON 12 I2c_Rx_Buf VAR BYTE[I2c_BUFLEN] Bank0 I2c_Tx_Buf VAR WORD[8] Bank0 I2c_Rx_Index VAR Byte Bank0 I2c_Tx_Index VAR Byte Bank0 I2c_Byte VAR BYTE Bank0 I2c_DataFlag Var Byte Bank0 I2c_DataBusy VAR I2c_DataFlag.0 I2c_NewData Var I2c_DataFlag.1 I2c_Flag Var Byte Bank0 ; I2c event occured I2c_Count VAR Byte Bank0 I2c_Temp VAR Byte Bank0 ; hold copy of SSPSTAT Bits SrcPtr VAR Byte Bank0 ' ---------------------------------- n var byte loopCount Var Byte A2Dval Var Word myData Var Byte[3] nBytes var Byte pIndex VAR Byte dTimer Var Byte gSetValue Var Word ' used in debugging & tuning gSetValue1 Var Word gSetValue2 Var Word ' ------------------------------------------ Accel_x VAR WORD Accel_y VAR WORD Accel_z VAR WORD Gyro_x VAR WORD Gyro_y VAR WORD X_Angle VAR Word Y_Angle VAR Word Swing_Angle Var Word Steer_Angle Var Word Gyro_Swing_Angle Var Word Gyro_Steer_Angle Var Word SA_Flag Var Byte ' ------------------------------------------ DutyCycle Var Byte TargetSpeed VAR WORD CrntSpeed VAR Word NewSpeed VAR Word SpeedIncAmt VAR Byte SpeedIncFlag Var BIT ServoPulseTime Var Word SteerLimit VAR Word TargetSteer Var Word NewSteer VAR Word CrntSteer VAR Word SteerIncAmt VAR Byte SteerIncFlag Var BIT SteerDirFlag Var BIT ' ------------------------------------------ VelFlag Var Bit TargetVel Var Word ' Velocity Velocity Var Word prevVelo Var Word P_Error VAR Word I_Error VAR Word D_Error VAR Word mBias VAR Word sumError Var Word vError Var Word ' ------------------------------------------ NewCmdFlag Var BIT theCmd Var Byte theData Var Word theResult Var Word AxVal Var Word AyVal Var Word tmpByte Var Byte tmpWord VAR WORD theSetting VAR Word prevCount Var Byte PWM_Flag VAR BIT SignBit Var Bit LogON_Flag Var Bit '-------------------------------------------------------------------------- CodeStart: GOTO Init_Seq '============================================================================ '============================================================================ ' Do_ISR ' Interupt Service Routine ' Handle all the Interupts 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 ; Save FSR register... MOVF FSR,W MOVWF fSave ; ========================================================================== ; See who caused the Interupt. ; Vector to the correct routines CLRF STATUS ; change to bank 0 regardless of current bank 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 BTFSC PIR1,SSPIF ; I2C Event ? GOTO I2C_INT ; Yes - go to I2C routine CLRF PIR1 GOTO ISR_Done ; Error - never handled interupt ; =========================================================================== ; Timer1 Interupt ; Controls Servo Pulse Widths and 10ms Timer. ; There are 3 different timer states to each 20ms frame: ; PulseON, PulseOFF, 10msWait ; TickTimer Clock gets inc'd every 10ms, at pulse ON, and start of 10ms blank T1_INT ; Timer1 Overflow BCF T1CON,TMR1ON ; Turn OFF timer1 BCF PIR1, TMR1IF ; Clear the interupt flag BTFSC _ServoPin ; Is Pin currently High? Goto TurnOFF ; Yup - turn off Pulse - reload w/OFF timer BTFSS _Wait10msFlag ; Is it time to turn on Servo Pulse? Goto TurnON ; jump to turn on servo pulse BCF _Wait10msFlag ; Next interupt is start of Servo Pulse MOVF _T1_10ms_VALUE,W ; Reload 10ms value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _T1_10ms_VALUE+1,W ADDWF TMR1H,F IncTickTimer INCF _TickTimer,F ; TickTimer gets inc'd every 10 ms BTFSC STATUS,Z ; 65534 Ticks = 10.9 minutes INCF _TickTimer+1,F ; 256 ticks = 2.56 seconds GOTO Timer1Done ; 1 second = 100 Ticks ; -------------------------------------------- ; Start 1.5ms Servo Pulse TurnON BSF _ServoPin ; Set Servo Pin High BCF _Wait20msFlag ; Tell foreground 20ms is up. MOVF _Pulse_ON_Time,W ; Load Pulse_ON value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _Pulse_ON_Time+1,W ADDWF TMR1H,F GOTO IncTickTimer ; -------------------------------------------- ; Servo Pulse is High - Turn it OFF ; Reload Timer-1 with OFF time to next 10ms frame ; next event is 10ms wait. TurnOFF BCF _ServoPin ; Turn Pin OFF MOVF _Pulse_OFF_Time,W ; Load Pulse_OFF value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _Pulse_OFF_Time+1,W ADDWF TMR1H,F BSF _Wait10msFlag ; Next interupt is just 10ms wait Timer1Done BSF T1CON,TMR1ON ; Turn ON timer1 ; GOTO ISR_Done ; ------------------------------------------------------------ ISR_Done Movf fSave,w ; Restore the FSR reg Movwf FSR 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 ; ============================================================================ ; Get the Quadrature Encoder Data ; Inc/Dec QuadCount during interupts @ 1.25 kHz sample rate ; Clear QuadCount during steering cycle T2_INT ; Handle Timer2 / PWM Interupt BCF PIR1, TMR2IF ; Clear the interupt flag ; GOTO QUAD_CHECK ; =============================== ; Scan the Quad Encoder Pins at 1.25khz ; Keep track of prev state, Inc or Dec QuadCount if wheel is spinning QUAD_CHECK MOVF PORTB,W ; Read Quad Sensor Pins - PortB Pins 1&2 MOVWF _QuadPins ; data in slots 0110 RRF _QuadPins,F ; Roll right -> 0011 MOVLW 3 ; only want bottom 2 slots ANDWF _QuadPins,F ; AND with 0011 MOVF _QuadPins,W XORWF _QuadPrev,W ; see if they're the same BTFSC STATUS,Z GOTO ISR_DONE ; prev pins same as current - no change SUBLW 3 ; see if both pins changed BTFSC STATUS,Z GOTO Quad_Error ; shouldn't happen BCF STATUS,C ; Clear carry bit RRF _QuadPrev,W XORWF _QuadPins,W ; Check Direction bits... MOVWF _QuadPrev BTFSS _QuadPrev,0 GOTO Dec_Count ; Dec Count -1 INCF _QuadCount,F ; Inc Count +1 Quad_Done MOVF _QuadPins,W MOVWF _QuadPrev ; save new pin state GOTO ISR_DONE Dec_Count DECF _QuadCount,F GOTO Quad_Done Quad_Error INCF _QuadError,F GOTO Quad_Done EndASM '------------------------------------------------------------------ Include "I2c_Slave_Asm.inc" '------------------------------------------------------------------ include "umfpu_NoDebug.inc" ' uM-FPU support routines '------------------------------------------------------------------ ' Subroutines... '------------------------------------------------------------------ ' Load FPU with inital values ' dT = 0.02 for 50Hz gyro integration Initialize_FPU: SHIFTOUT FpuOut, FpuClk, MSBFIRST, [dT, ATOF, "0.02", 0, FSET] Swing_Angle = 28 Steer_Angle = 0 tmpWord = 464 ' Gyro_Bias SHIFTOUT FpuOut, FpuClk, MSBFIRST, [_ fpuSwingAngle, LOADWORD, Swing_Angle\16, FSET,_ fpuSteerAngle, LOADWORD, Steer_Angle\16, FSET,_ SwingTilt, FSET+fpuSwingAngle,_ SteerTilt, FSET+fpuSteerAngle,_ x_bias, LOADWORD, tmpWord\16, FSET,_ y_bias, LOADWORD, tmpWord\16, FSET] GOSUB Fpu_Wait RETURN '------------------------------------------------------------------------------- '------------------------------------------------------------------------------- '------------------------------------------------------------------------------- ' Enter with theSetting = [-1000..0..1000] ' Full Fwd, Full Stop, and Full Rev turn PWM Off ' Set_Motor_Speed: CrntSpeed = theSetting + 1000 ' CrntSpeed range is [0..1000..2000] If theSetting == 0 Then ' Full Stop CCP1CON = 0 ' PWM mode OFF PWM_Flag = 0 fwdpin = 0 revpin = 0 EnaPin = 0 Return EndIf ' --- tmpWord = theSetting + 1000 ' range is now [0..1000..2000] If tmpWord == 0 Then ' Full Reverse CCP1CON = 0 ' PWM mode OFF PWM_Flag = 0 fwdpin = 0 revpin = 1 EnaPin = 0 Return EndIf If tmpWord == 2000 Then ' Full Forward CCP1CON = 0 ' PWM mode OFF PWM_Flag = 0 fwdpin = 1 revpin = 0 EnaPin = 0 Return EndIf ' --- tmpWord = (tmpWord * 32) / 125 ' convert range +- 1000 to +- 256 ' --- if tmpWord > 256 then ' range is now [0..256..512] tmpWord = tmpWord - 256 fwdpin = 1 revpin = 0 else tmpWord = 256 - tmpWord fwdpin = 0 revpin = 1 Endif tmpWord = 256 - tmpWord ; For PWM, 0 = full speed ; flip so range is 0..255..0 ; reverse dir in center CCPR1L = tmpWord MIN 255 ; Set Duty Cycle 0..255 If PWM_Flag == 0 Then ; Make sure PWM is On CCP1CON = %00101100 ' PWM mode, 2 LSBs of duty cycle = 10 PWM_Flag = 1 EndIf RETURN ' ------------------------------------------------------------------------------ ' Enter with theSetting set to the Target Speed you want to get to. ' System will accelerate (or deaccelerate) smoothly until it hits ' the target speed. ' Motor Speed Range is [-256..0..256] ' Target, and CrntSpeed are kept in range [0..1000..2000] ' Enter with theSetting in the range [-1000..0..1000] Set_Target_Speed: TargetSpeed = theSetting + 1000 TargetSpeed = TargetSpeed MIN 2000 TargetSpeed = TargetSpeed Max 0 If TargetSpeed == CrntSpeed Then SpeedIncFlag = 0 SpeedIncAmt = 0 RETURN ENDif If TargetSpeed >= CrntSpeed Then SpeedIncFlag = 1 SpeedIncAmt = 16 Else SpeedIncFlag = 0 ' Decrament Speed to reach target SpeedIncAmt = 16 Endif ' SpeedIncAmt = (Abs (TargetSpeed - CrntSpeed)) / 100 ' If SpeedIncAmt == 0 then SpeedIncAmt = 1 ' IF SpeedIncAmt > 50 Then SpeedIncAmt = 50 RETURN ' ------------------------------------------------------------------------------ ' Slowly change the Motor Speed... ' This gets called 50 times per second in the main loop ' If we are accelerating, this is where it happens ' Inc/Dec the current speed, send cmd to motor control ' If SpeedIncAmt == 0 nothing happens. ' NewSpeed, CrntSpeed & TargetSpeed all in range [0..1000..2000] Speed_Control_Loop: IF VelFlag Then ' prevent run-away If (Velocity == TargetVel) Then TargetSpeed = CrntSpeed ' IF (Velocity > TargetVel) AND (TargetSpeed > CrntSpeed) Then ' TargetSpeed = (CrntSpeed - 100) Max 1000 ' SpeedIncAmt = 10 ' SpeedIncFlag = 0 ' EndIf EndIf If SpeedIncAmt Then If SpeedIncFlag Then If (CrntSpeed >= 1000) AND (CrntSpeed < 1200) Then NewSpeed = CrntSpeed + 100 Else NewSpeed = CrntSpeed + SpeedIncAmt EndIf If NewSpeed >= TargetSpeed Then NewSpeed = TargetSpeed SpeedIncAmt = 0 EndIf Else SpeedIncAmt = 12 NewSpeed = CrntSpeed - SpeedIncAmt If NewSpeed.Bit15 Then NewSpeed = 0 ' don't go negative If NewSpeed <= TargetSpeed Then NewSpeed = TargetSpeed SpeedIncAmt = 0 EndIf EndIf If NewSpeed.Bit15 Then NewSpeed = 0 NewSpeed = NewSpeed Min 2000 ' NewSpeed in [0..1000..2000] theSetting = NewSpeed - 1000 ' Motor_Speed wants [-1000..0..1000] Gosub Set_Motor_Speed EndIf Return ' ------------------------------------------------------------------------------ ' Called from Main loop 5 times per second to adjust speed based on Velocity ' This is really a PID control function. Check_Velocity: ' IF VelFlag And (Velocity <> TargetVel) AND (CrntSpeed == TargetSpeed) Then IF VelFlag AND (CrntSpeed == TargetSpeed) Then mBias = 200 If TargetVel > 15 Then mBias = 200 + ((TargetVel - 15) * 3) EndIf tmpWord = prevVelo - Velocity ' Calc delta for derivitive D_Error = (tmpWord * gSetValue) / 10 ' derivitive of velocity = acceleration vError = TargetVel - Velocity sumError = sumError + vError If (Abs sumError) > 5000 Then ' Limit sumError to 5% If sumError.Bit15 Then sumError = -5000 Else sumError = 5000 EndIf EndIf SignBit = sumError.Bit15 I_Error = (Abs sumError) / 1000 If SignBit Then I_Error = 0 - I_Error ' --- Speeding up takes longer than slowing down, so 2 different functions If (Velocity < TargetVel) Then ' We need to speed up P_Error = vError * 2 TargetSpeed = (1000 + (mBias + P_Error + I_Error + D_Error)) Min 2000 If TargetSpeed.Bit15 Then TargetSpeed = 1000 TargetSpeed = TargetSpeed Min 2000 TargetSpeed = TargetSpeed Max 1000 SpeedIncAmt = 4 SpeedIncFlag = 1 ' Increment Speed to reach target TargetSpeed = TargetSpeed Min (CrntSpeed + 250) HSerOut [9, "-*- Inc Velocity - New TargetSpeed: ", Dec TargetSpeed,_ 9, "Delta: ", sdec tmpWord, 9, "mBias: ", Dec mBias, 9, "sumErr: ", SDec sumError,_ 9, "P_Err: ", SDEC P_Error, 9, "D_Err: ", SDEC D_Error,_ 9, "I_Err: ", SDEC I_Error, 9, "P+I+D: ", sDec (P_Error + I_Error + D_Error)] ELSE If (Velocity > TargetVel) Then ' We need to slow down P_Error = vError * 1 D_Error = (tmpWord * gSetValue) / 5 TargetSpeed = (1000 + (mBias + P_Error + I_Error + D_Error)) Min 2000 If TargetSpeed.Bit15 Then TargetSpeed = 1000 TargetSpeed = TargetSpeed Min 2000 TargetSpeed = TargetSpeed Max 1000 TargetSpeed = TargetSpeed Max (CrntSpeed - 250) SpeedIncAmt = 300 SpeedIncFlag = 0 ' Decrement Speed to reach target HSerOut [9, "-*- Dec Velocity - New TargetSpeed: ", Dec TargetSpeed,_ 9, "Delta: ", sdec tmpWord, 9, "sumErr: ", SDEC sumError,_ 9, "P_Err: ", SDEC P_Error, 9, "D_Err: ", SDEC D_Error,_ 9, "I_Err: ", SDEC I_Error, 9, "P+I+D: ", sDec (P_Error + I_Error + D_Error) ] EndIf EndIf ' --- EndIf prevVelo = Velocity ' to calc derivitive RETURN ' ------------------------------------------------------------------------------ ' Enter with theSetting in [-1000..0..1000] ' Internal range = [0..1000..2000] Set_Target_Steer: ' HSerOut [13, "Set_Target_Steer: ", sdec theSetting, " Crnt: ", sdec CrntSteer, 13, 13] TargetSteer = theSetting + 1000 TargetSteer = TargetSteer MIN 2000 TargetSteer = TargetSteer Max 0 If TargetSteer == CrntSteer Then SteerIncFlag = 0 SteerIncAmt = 0 RETURN ENDif If TargetSteer >= CrntSteer Then SteerIncFlag = 1 Else SteerIncFlag = 0 ' Decrament Speed to reach target Endif ' SteerIncAmt = (Abs (TargetSteer - CrntSteer)) / 100 ' If SteerIncAmt == 0 then SteerIncAmt = 1 ' IF SteerIncAmt > 10 Then SteerIncAmt = 10 SteerIncAmt = 20 ' HSerOut [ "End of Target_Steer: ", sdec TargetSteer,_ ' " Crnt: ", sdec CrntSteer, " Inc: ", sdec SteerIncAmt, 13] RETURN ' ------------------------------------------------------------------------------ ' v7 Steering Range [-1000..0..1000] ' Internal [0..1000..2000] Steer_Control_Loop: If SteerIncAmt Then If SteerIncFlag Then NewSteer = CrntSteer + SteerIncAmt If NewSteer >= TargetSteer Then NewSteer = TargetSteer SteerIncAmt = 0 EndIf Else NewSteer = CrntSteer - SteerIncAmt If NewSteer.Bit15 Then NewSteer = 0 ' don't go negitive If NewSteer <= TargetSteer Then NewSteer = TargetSteer SteerIncAmt = 0 EndIf EndIf If NewSteer.Bit15 Then NewSteer = 0 NewSteer = NewSteer Min 2000 ' NewSteer in [0..1000..2000] theSetting = NewSteer - 1000 ' Set_Steering wants [-1000..0..1000] Gosub Set_Steering EndIf Return '------------------------------------------------------------------------------- ' Set_Steering: Enter with theSetting = [-1000..0..1000] ' Setup the actual pulse width [ 900...1500...2100 ] ' Steering is limited to 52% so counter weight doesn't hit motor ' Input ranges is still +- 100% ' 100% as Input produces 52% travel of arm. (to limit of travel) Set_Steering: ; HSerOut ["Set Steering: ", SDec theSetting, 13] SignBit = theSetting.Bit15 CrntSteer = theSetting + 1000 ' CrntSteer in range [0..1000..2000] theSetting = ABS theSetting ' theSetting = theSetting Min 520 ' Limit SwingArm Travel to 52% ' theSetting = (6 * theSetting) / 10 ' Servo pulse is 1500 +- 600 theSetting = (theSetting * 32) / 100 theSetting = theSetting Min 312 ' Limit SwingArm Travel to 52% of 600 If SignBit Then ServoPulseTime = 1500 - theSetting Else ServoPulseTime = 1500 + theSetting EndIf ; Gosub Setup_Timers ; RETURN '------------------------------------------------------------------------------- ' Setup the Timer-1 Values to trigger 1.5ms Servo Pulses ' These are loaded into Timer-1 within the Interupt Asm routines. ' Pulse ON and OFF times must add up to 10ms '------------------------------------------------------------------------------- Setup_Timers: ServoPulseTime = ServoPulseTime MIN 2100 ServoPulseTime = ServoPulseTime MAX 900 tmpWord = (ServoPulseTime * 5) / 2 ' ServoPulseTime = [900..1500..2100] Pulse_ON_Time = 65535 - tmpWord ' 1500 = 1.5ms Pulse Width Pulse_OFF_Time = 65535 - (25000 - tmpWord) ' 25k = 10ms RETURN '=============================================================================== ' 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 SHIFTOUT FpuOut, FpuClk, MSBFIRST,[_ rate, LOADWORD, Gyro_x\16, FSET, FSUB+x_bias,_ SELECTA, FSET+rate, FMUL+dT, fpuSwingAngle, FADD] SHIFTOUT FpuOut, FpuClk, MSBFIRST,[_ rate, LOADWORD, Gyro_y\16, FSET, FSUB+y_bias,_ SELECTA, FSET+rate, FMUL+dT, fpuSteerAngle, FADD] GOSUB Fpu_Wait RETURN '------------------------------------------------------------------------------- ' Update angle and bias registers ' Enter with SA_Flag equal to 0 or 1 for Swing_Angle or Steer_Angle ' Enter with theSetting equal to either Swing_Angle or Steer_Angle Adjust_Bias: ' Angle_Err = Tilt_Angle - Gyro_Angle ' angle += (0.2 * angle_err); ' q_bias += (0.08 * angle_err); SHIFTOUT FpuOut, FpuClk, MSBFIRST,[_ Angle_Err, LOADWORD, theSetting\16, FSET, FSUB+fpuSwingAngle+SA_Flag] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Err, ATOF, "0.2", 0, FSET,_ tmp, FSET+Angle_Err, FMUL+Err, fpuSwingAngle+SA_Flag, FADD+tmp] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Err, ATOF, "0.08", 0, FSET,_ tmp, FSET+Angle_Err, NEGATE, FMUL+Err, x_bias+SA_Flag, FADD+tmp] GOSUB Fpu_Wait ' ... now the new angle and bias values are available ' FIX puts Int value of A-reg into register zero SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+tmp, FSET+fpuSwingAngle+SA_Flag,_ ROUND, FIX, SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [theResult.HIGHBYTE, theResult.LOWBYTE] If theResult == -14081 then theResult = theSetting ' FPU read error RETURN '------------------------------------------------------------------------------- ' Steering Angle tilt uses the Y and Z axis readings ' Swing Angle uses the X and Z axis readings ' Enter with raw readings from Accelerometers [0..512..1023] ' Result is adjusted into range with zero as steady resting state. ' Result Angles are plus minus degrees from zero. [ -180..0..180 ] Calc_Tilt_Angle: AxVal = AxVal - 512 AyVal = AyVal - 512 SHIFTOUT FpuOut, FpuClk, MSBFIRST,_ [xVal, LOADWORD, AxVal\16, FSET,_ yVal, LOADWORD, AyVal\16, FSET,_ SELECTA+yVal, SELECTB+xVal, XOP, ATAN2, DEGREES,_ SELECTA+yVal, ROUND, FIX] ' FIX puts Int value of A-reg into register zero GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [theResult.HIGHBYTE, theResult.LOWBYTE] theResult = theResult - 90 RETURN '------------------------------------------------------------------------------- ' Subroutines to read AD converter voltages Read_AD: Pauseus 10 ' Wait for channel to setup ADCON0.2 = 1 ' Start conversion wLoop: Pauseus 5 ' Wait for conversion IF ADCON0.2 = 1 then wLoop A2Dval.HIGHBYTE = ADRESH A2Dval.LowBYTE = ADRESL Return '----------------------------------------------------- ' Subroutines to read IMU 5 dof accelerometer voltages Read_Accel_X: ADCON0 = %10000001 ' Set A/D to Fosc/32, Channel 0, On Goto Read_AD Read_Accel_Y: ADCON0 = %10001001 ' Set A/D to Fosc/32, Channel 1, On Goto Read_AD Read_Accel_Z: ADCON0 = %10010001 ' Set A/D to Fosc/32, Channel 2, On Goto Read_AD Read_Gyro_X: ADCON0 = %10100001 ' Set A/D to Fosc/32, Channel 4, On Goto Read_AD Read_Gyro_Y: ADCON0 = %10101001 ' Set A/D to Fosc/32, Channel 5, On Goto Read_AD Read_IMU_vRef: ADCON0 = %10110001 ' Set A/D to Fosc/32, Channel 6, On Goto Read_AD '=============================================================================== ' Do a rolling average of the previous 6 readings. ' We only get 36 clicks per revolution - 1 per inch rolled ' So if we're polling at 50 hz, we see 49 zeros and 1 click per second. ' So we poll at 5 hz, and do this rolling average, to try to track low speeds. Calc_Velocity: for n = 0 to 4 QStack[n] = QStack[n+1] Next n QStack[5] = QuadCount QuadCount = 0 If QStack[5] then Toggle LED tmpWord = 0 for n = 0 to 5 tmpByte = QStack[n] If tmpByte.Bit7 Then tmpWord = tmpWord - (Abs tmpByte) else tmpWord = tmpWord + tmpByte EndIf Next n SignBit = tmpWord.Bit15 Velocity = ((abs tmpWord) * 10) / 6 ' times 10 .. so 15 = 1.5 ips if SignBit then Velocity = 0 - Velocity EndIf RETURN '=============================================================================== ' Read_All_Sensors - Read all the IMU sensors and integrate the results. ' Calc the Tilt angles and track the gyros. ' Run Accel readings thru low pass filter before integrating. Read_All_Sensors: Gosub Read_Gyro_X : Gyro_X = A2Dval Gosub Read_Gyro_Y : Gyro_y = A2Dval Gosub Integrate_Gyro Gosub Read_Accel_X : Accel_X = (A2Dval + (Accel_X * 3)) / 4 Gosub Read_Accel_Y : Accel_y = (A2Dval + (Accel_Y * 3)) / 4 Gosub Read_Accel_Z : Accel_Z = (A2Dval + (Accel_Z * 3)) / 4 AxVal = Accel_X AyVal = Accel_Z Gosub Calc_Tilt_Angle Swing_Angle = theResult ' Natural balance point = 28 degrees AxVal = Accel_Y AyVal = Accel_Z Gosub Calc_Tilt_Angle Steer_Angle = theResult ' straight steering around 30 SA_Flag = 0 theSetting = Swing_Angle Gosub Adjust_Bias ' Calc new Swing and Steering Angles Gyro_Swing_Angle = theResult SA_Flag = 1 theSetting = Steer_Angle Gosub Adjust_Bias ' Calc new Swing and Steering Angles Gyro_Steer_Angle = theResult ' If dTimer == 5 then ' HSerOut [ "Tilt Swing: ", SDEC Swing_Angle, " Gyro Swing: ", SDEC Gyro_Swing_Angle, " " ] ' HSerOut [ "Tilt Steer: ", SDEC Steer_Angle, " Gyro Steer: ", SDEC Gyro_Steer_Angle, 13 ] ' EndIf RETURN '------------------------------------------------------------------------------- ' Copy Device Variables into I2c Communication Area ' Double buffer the data to prevent values being read while writing ' Update_I2c_Data: I2c_DataBusy = 1 ' Tell asm table 1 is busy - or turn intrps OFF I2c_Tx_Buf[0] = TickTimer I2c_Tx_Buf[1] = CrntSpeed - 1000 I2c_Tx_Buf[2] = CrntSteer - 1000 I2c_Tx_Buf[3] = 2 I2c_NewData = 1 I2c_DataBusy = 0 ' Tell asm table 1 is read-able RETURN ' ------------------------------------------------------------------------------ ' When this device receives a command, it comes with two parts: ' theCmd has the command #, and theData contains the message. ' Check to see if there are at least 3 chrs in the Circular buffer ' If so, move 3 of them into theCmd/theData and process. ' Inc pIndex to move the back pointer in the circular buffer. ' Check_I2c_Commands: If I2c_Rx_Index == pIndex Then nBytes = 0 Else If I2c_Rx_Index > pIndex Then nBytes = I2c_Rx_Index - pIndex Else nBytes = (I2c_BUFLEN - pIndex) + I2c_Rx_Index EndIf EndIf If nBytes < 3 then RETURN For nBytes = 0 to 2 myData[nBytes] = I2c_Rx_Buf[pIndex] pIndex = pIndex + 1 If (pIndex >= I2c_BUFLEN) Then pIndex = 0 Next nBytes theCmd = myData[0] theData.HighBYTE = myData[1] theData.LowByte = myData[2] HSerOut [ 13, 13,_ "--Time: ", dec TickTimer,_ " theCmd: ", dec theCmd,_ " theData: ", sdec theData, 13] Toggle LED ' ----- ' If theCmd == 0 Then ' Full Stop ' theSetting = 0 ' Gosub Set_Motor_Speed ' TargetSpeed = CrntSpeed ' Reset Accelerator ' CrntSteer = 0 ' EndIf If TheCmd == SET_SPEED Then ' Change speed smoothly - set target speed if ((ABS theData) <= 100) Then theSetting = theData * 10 ' theData in [-100.0 .. 0 .. 100.0] VelFlag = 0 TargetVel = 0 sumError = 0 Gosub Set_Target_Speed EndIf EndIf If TheCmd == SET_STEER Then ' Smoothly Set Steering if ((ABS theData) <= 100) Then theSetting = theData * 10 Gosub Set_Target_Steer EndIf EndIf If TheCmd == SET_VELOCITY Then ' Smoothly Set Velocity if ((ABS theData) <= 100) Then theSetting = theData * 10 Gosub Set_Target_Speed VelFlag = 1 TargetVel = theData sumError = 0 EndIf EndIf If TheCmd == LOG_ON_OFF Then ' Turn Log File ON/OFF LogON_Flag = theData EndIf If TheCmd == SET_VALUE Then ' Set Misc value for testing & tuning gSetValue = theData EndIf If TheCmd == SET_VALUE1 Then gSetValue1 = theData EndIf If TheCmd == SET_VALUE2 Then gSetValue2 = theData EndIf ' If theCmd == 3 Then ' Set Motor Speed Directly [-256..0..256] ' theSetting = theData ' Gosub Set_Motor_Speed ' TargetSpeed = CrntSpeed ' EndIf ' If TheCmd == 4 Then ' Set Steering Directly ' theSetting = theData ' EndIf Return ' ------------------------------------------------------------------------------ Blip_LED: High LED Pause 20 Low LED RETURN ' ------------------------------------------------------------------------------ ' Loop here for-ever. ' Asm routines clr the 20ms flag every 20ms (dah) ' Go thru each of the tasks that needs to be done every 20ms. ' waitLoop: If Wait20msFlag then ' If set, waste time & stall here Gosub Check_I2c_Commands Goto waitLoop ' loop until asm clears the flag EndIf Wait20msFlag = 1 'reset - asm will clear the flag every 20ms Gosub Read_All_Sensors Gosub Update_I2c_Data Gosub Check_I2c_Commands Gosub Speed_Control_Loop Gosub Steer_Control_Loop Gosub Update_I2c_Data ' so data is always current IF (Abs Swing_Angle) > 160 Then ' prevent roll-over theSetting = 0 Gosub Set_Motor_Speed TargetSpeed = CrntSpeed ' Reset VelFlag = 0 EndIf dTimer = dTimer + 1 If dTimer > 9 Then Gosub Calc_Velocity ' Called 5 times per second ' GOSUB Check_Velocity ' Adjust target speed If LogON_Flag then HSerOut [ 13,_ "Time: ", dec TickTimer,_ 9,"StrA: ", SDEC Steer_Angle,_ 9,"GSwgA: ", sdec Gyro_Swing_Angle,_ 9,"CurVelo: ", sdec Velocity,_ 9,"TarVelo: ", sdec TargetVel,_ 9,"CurSpd: ", sdec (CrntSpeed - 1000),_ 9,"TarSpd: ", sdec (TargetSpeed - 1000),_ 9,"sumErr: ", sdec sumError,_ 9, 9,"RAW: ", dec Accel_x,_ ", ", dec Accel_y,_ ", ", dec Accel_z,_ ", ", dec Gyro_x,_ ", ", dec Gyro_y ] EndIf dTimer = 1 EndIf Goto waitLoop ' ---------------------------------------------------------------- mainLoop: Gosub Blip_LED loopCount = 0 dTimer = 0 goto waitLoop ' ---------------------------------------------------------------- ShutDown: CCP1CON = 0 LOW FwdPin LOW RevPin LOW EnaPin LOW LED END ' ---------------------------------------------------------------- ' ---------------------------------------------------------------- ' ---------------------------------------------------------------- Init_Seq: Pause 50 ' Give PIC time to settle TRISA = %00111111 ' Set PORTA to some input TRISB = %00000110 ' Set PORTB for Quad Encoder Inputs TRISC = %00011000 ' Set PORTC TRISD = %00000000 ' Set PORTD to all output ADCON1 = %10000011 ' Set AD clock to Foxc/64, AD uses A3 as +VRef ADCON0 = $80 ' Set A/D to Fosc/32, Channel 0, Off T1CON = %00010000 ; Timer1 1:2 pre-scaler, Timer1 OFF - Servo Pulses TMR1H = 255 TMR1L = 254 TMR1ON = 1 ; Turn Timer1 ON T2CON = %00000011 ' Timer2 Pre-Scaler = 1:16, Timer OFF - Quad Encoder TMR2 = 0 ' Clear Timer2 PR2 = 255 ' Setup PWM Period = 1.25 kHz TMR2ON = 1 ' Turn Timer2 ON ' --- SSPADD = $08 ' I2c Slave Device Address SSPCON = $36 ' Setup I2c Slave for 7-bit address SSPSTAT = 0 CCP1CON = 0 ' CCP Module OFF PWM_Flag = 0 LOW FwdPin LOW RevPin LOW EnaPin Input QuadPin1 Input QuadPin2 I2c_Flag = 0 I2c_Rx_Index = 0 I2c_Tx_Index = 0 I2c_Count = 0 pIndex = 0 I2c_DataFlag = 0 QuadCount = 0 QuadPrev = 0 QuadError = 0 QuadPrev.0 = QuadPin1 QuadPrev.1 = QuadPin2 Velocity = 0 prevVelo = 0 VelFlag = 0 TargetVel = 0 sumError = 0 for n = 0 to 5 QStack[n] = 0 Next n ServoFlags = 0 NewCmdFlag = 0 prevCount = 0 LogON_Flag = 0 gSetValue = 1 TickTimer = 0 T1_10ms_VALUE = 65535 - 25000 ' 10 ms at 20 mHz w/ 1:2 prescaler ServoPulseTime = 1500 ' 1.5 ms = Servo Center Position Gosub Setup_Timers TargetSpeed = 1000 CrntSpeed = 1000 TargetSteer = 1000 ' Center is 1000 internal CrntSteer = 1000 ' Range is [-1000..0..1000] Accel_X = 512 ' Init so Avg starts out ok Accel_Y = 512 Accel_Z = 600 gSstValue = 0 gSetValue1 = 12 gSetValue2 = 16 Banner: HSEROUT [13, "Loco-Motion Module v12", 13] HSEROUT ["Uses Swing Angle PID function...", 13] ' Reset the uM-FPU Floating Point processor GOSUB Fpu_Reset 'reset the FPU hardware IF fpu_status <> SyncChar THEN HSEROUT ["uM-FPU not detected.",13] END ENDIF HSEROUT ["FPU Looks alive...", 13] Gosub Initialize_FPU Pause 1000 Low LED GIE = 1 ' Global Interupt Enable PEIE = 1 ' Peripheral Interupt Enable TMR1IE = 1 ' Timer1 Interupt Enable bit - Servo 1.5 ms Pulse TMR2IE = 1 ' Timer2 Interupt Enable bit - Quad Wheel Encoder SSPIE = 1 ' I2c Interupts '----------------------------------------------------------------------- '----------------------------------------------------------------------- '----------------------------------------------------------------------- HSEROUT ["Interupts are ON, go to Main Loop...", 13, 13] GOTO mainLoop ' ------------------------------------------------------------------------------ ' End of File