#macro EnterSleepMode()
low D.7 'STSPIN220 EN low to save power and reduce heat
low A.5 'DRB8837 EN low to save power
hi2cout %00010000, (0x00,0x00,0x40) 'EN3V3 low to disable 3.30V switcher and save power
disablebod 'Disable brown-out detect for extra power savings
sleep 0 'Sleep for infinity to save power
#endmacro
{'Bit variables
Symbol sleeping = bit31 'Flag for sleep mode
}
{'Byte variables
Symbol b_counter = b4
Symbol mag_cmd = b5
}
{'Wrod variables
Symbol sum0 = w27 'b54:b53; Additions
Symbol l_error = w26 'b52:b51; Line error
Symbol chg_sense = w25 'b50:b49; Line error
Symbol speed = w24 'b48:b47; Line error
Symbol helper = w23 'b46:b45; Line error
Symbol chg_voltage = w22 'b44:b43; Line error
Symbol LL_ADC = w21 'b42:b41; Leftmost hall sensor reading
Symbol L_ADC = w20 'b40:b39; Left hall sensor reading
Symbol R_ADC = w19 'b38:b37; Right hall sensor reading
Symbol RR_ADC = w18 'b36:b35; Rightmost hall sensor reading
Symbol counter0 = w17 'b34:b33; Just a counter
Symbol counter1 = w16 'b32:b31; Just a counter
Symbol ELine = w15 'b30:b29; Line position error
Symbol PLine = w14 'b28:b27; P term, PID
Symbol DLine = w13 'b26:b25; D term, PID
Symbol Steer_input = w12 'b24:b23; Steer input, number of stp pulses to STSPIN220
Symbol D_error = w11 'b22:b21; Derivative error for D term calculation
Symbol OldELine = w10 'b20:b19; Old line position error
Symbol steer_sense = w9 'b18:b17;
Symbol max_steer_input = w8 'b16:b15; Agregated steering input
Symbol steer_pos = w7
Symbol steer_output = w6
Symbol line_pos = w0 'b0:b1; MagLine position
}
{'I2C addresses
Symbol PCA9551_R = 0xC6 'rear light panel
Symbol TCA6507 = 0x8a 'LED driver/output expander
}
{'Constants
Symbol Steer_pulse = 200 '200
Symbol steer_limit_left = 20 'ADC value with magnet centered on steering position sensor
Symbol steer_max = 380 'Could be the number of allowed steps from centre in one direction
Symbol steer_limit = 220'210
Symbol min_spread = 20 '20 'low treshold for considering adjacent hall sensors equal
Symbol max_spread = 35 '35 'max difference of adjacent hall sensors
Symbol KpELine = 26 '26 'P coefficient/multiplier for line position error correction
Symbol KdEline = 900 '900 'D coefficient for line error corrections
Symbol InputDiv = 30 '30 'Input divider to go down to reasonable value for number of steps for the motor
}
{'FREQ dependent constants
'Symbol I2CSpeed = i2cfast
Symbol I2CSpeed = i2cfast_64
}
{'PIC registers
Symbol PMD0 = $3F
Symbol PMD1 = $3E
Symbol PMD2 = $3D
Symbol TRIS_E = $96
Symbol TRIS_D = $95
Symbol TRIS_C = $94 'have to use underscore, TRISC variable taken by firmware
Symbol TRIS_B = $93
Symbol TRIS_A = $92
Symbol T1CON = $CD
Symbol T2CON = $BA
Symbol T4CON = $51
Symbol T6CON = $4A
Symbol TMR4 = $53
Symbol TMR5H = $50
Symbol TMR5L = $4F
Symbol T5CON = $4E
Symbol T5GCON = $4D
Symbol PR2 = $BB
Symbol PR4 = $52
Symbol PR6 = $4B
Symbol PIR1 = $9E
Symbol PIR2 = $A1
Symbol PIR3 = $A4
Symbol PIR4 = $7B
Symbol PIR5 = $7E
Symbol PIE1 = $9D
Symbol PIE2 = $A0
Symbol PIE3 = $A3
Symbol PIE4 = $7A
Symbol PIE5 = $7D
Symbol CCPR1H = $BF
Symbol CCPR1L = $BE
Symbol CCPR3H = $5F
Symbol CCPR3L = $5E
Symbol CCP3CON = $5D
Symbol CCP1CON = $BD
Symbol PWM1CON = $B7
Symbol PWM3CON = $5C
Symbol ECCP1AS = $B6
Symbol PSTR1CON = $B9
Symbol ECCP3AS = $5B
Symbol PSTR3CON = $5A
Symbol CCPR5H = $56
Symbol CCPR5L = $55
Symbol CCP5CON = $54
Symbol CCP2CON = $66
Symbol PSTR2CON = $63
Symbol CCPTMRS0 = $49
Symbol CCPTMRS1 = $48
}
setfreq em64 'Set frequency to 64MHz after exiting sleep mode
hintsetup %00000100 'Set up hardware interrupts for ON/OFF button
setintflags %00000100, %00000100 'Set up flags for hardware interrupts
hi2csetup i2cmaster, PCA9551_R, I2CSpeed, i2cbyte 'Set up I2C comms
EnterSleepMode 'Run sleep mode macro
main:
do
if sleeping = 0 then
EnterSleepMode 'Run sleep mode macro
else
gosub Init
do
gosub MagLine
if No_magnet = 0 then
GetSignedPLine 'Calculate P term
d_error = ELine - OldELine 'Calculate error trend (derivative)
GetSignedDLine 'Calculate D term
OldELine = ELine 'Set the old error the current error
GetLimitedSteerInput
RunLimitedSteeringStepper
endif
; gosub CheckForCharging
loop while sleeping <> 0
endif
loop
init:
enablebod 'Enable brown-out detect
hi2csetup i2cmaster, PCA9551_R, I2CSpeed, i2cbyte 'Set up I2C comms
'Initialise STSPIN220
high EN
hi2cout [TCA6507], %00010000, (0x00,0x00,0x10) 'STBY low
' high MODE1 'set high by pull-up
' low MODE2 'connected to GND on PCB
low DIR
high STCK
pause 100
hi2cout %00010000, (0x00,0x00,0x00) 'STBY set high by pullup to register state of mode pins
low STCK
low EN
InitSteering
sertxd ("out of init",CR,LF)
high A.5 'take DRV8837 out of sleep
'Initialise PWM, CCP3 on B.5, multiplexed by picaxe firmware /Main motor/
pwmout pwmdiv16, B.5, 249, 499 'period and duty values irrelevant as switching timer away from default timer4
input B.5 : input A.6 'make PWM pins inputs during configuration
pokesfr PR6, %00011111 'set timer6 period
'max speed for coil-finding
' pokesfr CCP3CON, %10111100 'configure half-bridge mode, active high polarity for both pins
' pokesfr CCPR3L, %00011010 'The rest of the duty cycle
'crawl speed
pokesfr CCP3CON, %10111100 'configure half-bridge mode, active high polarity for both pins
pokesfr CCPR3L, %01100111 'The rest of the duty cycle
'stop speed
' pokesfr CCP3CON, %10111100 'configure half-bridge mode, active high polarity for both pins
' pokesfr CCPR3L, %00001111 'The rest of the duty cycle
'magline chasing speed
' pokesfr CCP3CON, %10111100 'configure half-bridge mode, active high polarity for both pins
' pokesfr CCPR3L, %01011111 'The rest of the duty cycle
pokesfr PWM3CON, %10001010 'PRSEN bit set, but should not matter, dead-band delay set to 10
output B.5 : output A.6 'start PWM by enabling A and B pins as outputs
return
interrupt:
if pinPWRSW = 0 then interrupt
if hint2flag = 1 then 'if this is coming from the power switch...
if sleeping = 0 then 'was sleeping
sleeping = 1 'set sleeping flag to running state [1]
hint2flag = 0 : hintflag = 0 'reset interrupt flags
else 'was not sleeping
sleeping = 0 'set sleeping flag to sleeping state [0]
hint2flag = 0 : hintflag = 0 'reset interrupt flags
endif
endif
setintflags %00000100, %00000100 'enable interrupt flags
return