fl { PropForth5.5(DevKernel) RoverC-Pro 2020/12/29 17:38:51 RoverC-Pro Propeller GND ----------- GND 5V-IN G26(SDA)----------- SCL G36 G0(SDA) ----------- SDA BAT 3V3 ----------- 3V3 5V-OUT } : RoverC-Pro ; \ =========================================================================== \ Constants \ =========================================================================== \ Slave addres h38 h70 wconstant Rover variable Forward -4 allot d60 l, d60 l, d60 l, d60 l, variable Left -4 allot d-60 l, d60 l, d60 l, d-60 l, variable Back -4 allot d-60 l, d-60 l, d-60 l, d-60 l, variable Right -4 allot d60 l, d-60 l, d-60 l, d60 l, variable Rotate_R -4 allot d-100 l, d100 l, d-100 l, d100 l, variable Rotate_L -4 allot d100 l, d-100 l, d100 l, d-100 l, \ =========================================================================== \ Variables \ =========================================================================== wvariable f wvariable b wvariable l wvariable r wvariable rr wvariable rl \ For motor speed variable sendBuffer d12 allot \ =========================================================================== \ standard mode(100kHz) i2c Also SMBus \ =========================================================================== : err_msg ." I2C error" ; \ If error, print message \ ( n1 -- ) n1:t/f : err? if err_msg cr then ; \ Start i2c-commnication \ This also can use SMBus device. \ ( -- ) lockdict create _eestart forthentry $C_a_lxasm w, h122 h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z1[ixnW l, z1[ixnX l, z2WyP[U l, z20iPak l, z3ryPW0 l, z1bixnW l, z2WyP[V l, z20iPak l, z3ryPW0 l, z1bixnX l, z1SV01X l, zl0 l, zCW l, zW0000 l, zG0000 l, freedict \ Re-defined RepeatedStart \ ( -- ) : Sr _eestart ; \ Stop i2c-commnication \ ( -- ) : _eestop _scli \ Release scl _sdai \ Release sda ; \ std_eewrite ( c1 -- t/f ) write c1 to the eeprom, true if there was an error \ Received acknowledge from i2c-device during scl is high \ scl/sda use pull-up resistor at hi \ clock:100kHz lockdict create std_eewrite forthentry $C_a_lxasm w, h12C h113 1- tuck - h9 lshift or here W@ alignl h10 lshift or l, z2WyPW8 l, z1YVPQ0 l, z1rixnd l, z1Sy\C] l, z1[ixne l, z1Sy\C] l, z1bixne l, zfyPO1 l, z3[yP[K l, z1[ixnd l, z1Sy\C] l, z1[ixne l, z1Sy\C] l, z1YF\Nl l, z1viPR6 l, z1bixne l, z1Sy\C] l, z1bixnd l, z1SV01X l, z2WyPh3 l, z20iPik l, z3ryPb0 l, z1SV000 l, zW0000 l, zG0000 l, freedict \ Write data to register in i2c_device \ ( n1 n2 n3 -- ) n1:data n2:register n3:slave_address : std_i2c_wr \ Start I2C _eestart \ Write slave address[wr], then receive Acknowledge-bit(ACK:Lo NACK:Hi) std_eewrite \ ( n1 n2 t/f ) \ Write register swap std_eewrite or \ ( n1 t/f ) \ Write data swap std_eewrite or \ ( t/f ) \ Stop I2C _eestop err? \ ( -- ) ; \ Display connected i2c-device's slave-address \ This also can use SMBus device. \ ( -- ) : std_i2c_detect hex \ Print lower ." 0 1 2 3 4 5 6 7 8 9 A B C D E F" cr \ ( n1 n2 ) n1:reserve address 's count n2:i2c_device's count n3:i2c_address 0 0 0 8 0 do \ Print 1-line dup .byte h3A emit space d16 0 do \ Start I2C _eestart \ Write slave address[wr], then receive Acknowledge-bit(ACK:Lo NACK:Hi) dup i + 1 lshift std_eewrite 0= if \ there is ACK dup i + dup .byte \ Get slave-addres dup 7 > swap h78 < and \ 0 to 7 and h78 to h7F if swap 1+ swap \ Count up device (eliminate reserve number) else rot 1+ rot2 then else ." --" then space \ Stop I2C _eestop loop cr \ next line d16 + loop drop decimal ." i2c_device:" . cr 0> if ." [0 - 7] and [h78 - h7F] are reserve-address" then cr cr ; \ =========================================================================== \ Main \ =========================================================================== \ ( n1 n2 -- ) n1:data n2:register : writeRegister \ Start I2C _eestart \ Write slave address[wr], then receive Acknowledge-bit(ACK:Lo NACK:Hi) Rover std_eewrite swap std_eewrite or swap std_eewrite or \ Stop I2C _eestop err? ; { 50 0 writeRegister 50 invert 1+ 0 writeRegister 0 0 writeRegister 50 1 writeRegister 50 invert 1+ 1 writeRegister 0 1 writeRegister 50 2 writeRegister 50 invert 1+ 2 writeRegister 0 2 writeRegister 50 3 writeRegister 50 invert 1+ 3 writeRegister 0 3 writeRegister 50 0 writeRegister 50 1 writeRegister 50 2 writeRegister 50 3 writeRegister } \ Limit upper and lower \ ( n1 -- n2 ) n1:compared value n2:result : limit dup d100 > if drop d100 then dup d-100 < if drop d-100 then ; \ Set speed \ ( -- ) : setSpeed 4 0 do Forward i 4* + L@ f W@ * d127 / Back i 4* + L@ b W@ * d127 / Right i 4* + L@ r W@ * d127 / Left i 4* + L@ l W@ * d127 / Rotate_R i 4* + L@ rr W@ * d127 / Rotate_L i 4* + L@ rl W@ * d127 / + + + + + limit sendBuffer i 4* + L! loop \ sendBuffer 16 dump ; \ Drive RoverC-Pro \ ( -- ) : driveWheel 4 0 do sendBuffer i 4* + L@ i writeRegister loop ; { \ Test operation : demo \ Go forward d127 f W! setSpeed driveWheel begin fkey? swap drop until \ Go left 0 f W! d127 l W! setSpeed driveWheel begin fkey? swap drop until \ Go back 0 l W! d127 b W! setSpeed driveWheel begin fkey? swap drop until \ Go right 0 b W! d127 r W! setSpeed driveWheel begin fkey? swap drop until \ Stop 0 r W! setSpeed driveWheel begin fkey? swap drop until d127 f W! d127 r W! setSpeed driveWheel begin fkey? swap drop until 0 f W! d127 r W! d127 b W! setSpeed driveWheel begin fkey? swap drop until 0 r W! d127 l W! d127 b W! setSpeed driveWheel begin fkey? swap drop until d127 f W! d127 l W! 0 b W! setSpeed driveWheel begin fkey? swap drop until 0 f W! 0 l W! d127 rr W! setSpeed driveWheel begin fkey? swap drop until 0 rr W! d127 rl W! setSpeed driveWheel begin fkey? swap drop until 0 rl W! setSpeed driveWheel begin fkey? swap drop until ; }