' BFF_RBT_MASTER.bas ' Copyright 2008 BFF Design Ltd ' ' Ian Hopper (BFF Design Ltd) Aug 2008 ' http://buggies.builtforfun.co.uk - email Ian@builtforfun.co.uk ' ' For DIY Wi-Fi Remote Control Robot ' ' Drive 1 () - I2C Address 0xB2 ' Drive 2 () - I2C Address 0xB4 ' ' See the MD03 user documentation for the mode switch settings on the controller to set ' these addresses. ' --------------------------- ' ' For 28X1 A.3 chip using an external 16 MHz resonator.......... ' 'Declarations 'b1 to b8 are joystick buttons 1 to 8 'b11 to b13 are joystick x, y & z axes 'Assign the pan, speed and turn variables to suit your joystick mappings ' symbol v = b1 symbol pan_down = b2 'ie pan_down is joystick button 2 symbol pan_up = b3 symbol pan_left = b4 symbol pan_right = b5 ' symbol v = b6 ' symbol v = b7 ' symbol v = b8 symbol turn = b11 symbol speed = b12 'ie speed is joystick axis 2 (Y) ' symbol v = b13 'Don't alter these assignments below symbol s1 = w10 symbol sdir1 = b15 symbol s2 = w11 symbol sdir2 = b14 symbol sum1 = w12 symbol sum2 = w13 symbol speed2 = w9 'b0, b9, b10, b16 & b17 are used for return data 'Light the Amber LED switchon 1 'Set the chip frequency to get the 2-way 9600 baud serin to work with the 28X1 setfreq em16 'Wait for the start signal from the PC - the input values are ignored but the program 'will wait here until the PC sends a serial packet serin 7, N9600_16, ("AB"), b11, b12, b13, b1, b2, b3, b4, b5, b6, b7, b8 'Now setup the I2C comms with the MD03 controllers hi2csetup i2cmaster, $B2, i2cslow_16, i2cbyte hi2cout [$B2], 3, (50) 'write 50 to the accel register - set accel hi2cout 2, (0) 'write 0 to the speed register - set zero speed hi2cout 0, (0) 'write 0 to the command register - set instant stop at present hi2cout [$B4], 3, (50) 'write 50 to the accel register - set accel hi2cout 2, (0) 'write 0 to the speed register - set zero speed hi2cout 0, (0) 'write 0 to the command register - set instant stop at present pause 4000 '1 sec at 16 MHz switchoff 1 switchoff 4 switchoff 5 switchoff 6 switchoff 7 ' Now the main data processing loop - the loop time of the chip sets the loop time of the ' overall system MainLoop: speed = 50 turn = 50 'Read the input from PC on In7 (pin 18) - a 125ms timeout is set to detect loss of data from the PC switchoff 2 serin [500,tout], 7, N9600_16, ("AB"), b11, b12, b13, b1, b2, b3, b4, b5, b6, b7, b8 'From the speed and turn data calculate the individual speed and direction for each motor speed2 = speed + 950 'set a 950 to 1050 range with 1000 as zero point sum1 = speed2 + turn - 50 sum2 = speed2 - turn + 50 if sum1 < 1000 then s1 = 999 - sum1 s1 = s1 * 50 / 10 MIN 0 MAX 255 sdir1 = 1 else s1 = sum1 - 1000 s1 = s1 * 50 / 10 MIN 0 MAX 255 sdir1 = 2 endif if sum2 < 1000 then s2 = 999 - sum2 s2 = s2 * 50 / 10 MIN 0 MAX 255 sdir2 = 1 else s2 = sum2 - 1000 s2 = s2 * 50 / 10 MIN 0 MAX 255 sdir2 = 2 endif 'Switch off the green LED switchon 2 'Send these signals through to the speed controllers on the I2C bus hi2cout [$B2], 3, (50) 'write 50 to the accel register - set accel hi2cout 2, (s1) 'write s1 to the speed register - set s1 speed hi2cout 0, (sdir1) 'write sdir1 to the command register - set s1 direction hi2cout [$B4], 3, (50) hi2cout 2, (s2) hi2cout 0, (sdir2) 'Send the tilt and pan data through to the slave 28X1 if pan_up = 1 then switchon 7 else switchoff 7 endif if pan_down = 1 then switchon 6 else switchoff 6 endif if pan_left = 1 then switchon 5 else switchoff 5 endif if pan_right = 1 then switchon 4 else switchoff 4 endif 'Read the MD03 temps and currents, directions and speeds b9 = 0 b10 = 0 b16 = 0 b17 = 0 hi2cin [$B2], 4, (b9, b10) hi2cin 0, (sdir1, b0, s1) hi2cin [$B4], 4, (b16, b17) hi2cin 0, (sdir2, b0, s2) 'Write the MD03 status data back to the PC serout 3, N9600_16, ("P", b9, b10, b16, b17, sdir1, s1, sdir2, s2) ' Okay - back for another write-read loop goto MainLoop 'Come here if serin times out - ie no data from the PC - stop the drive tout: 'Important - close down the speed controllers if no data is being received from the PC PID controller. hi2cout [$B2], 3, (50) 'write 50 to the accel register - set accel hi2cout 2, (0) 'write 0 to the speed register - set zero speed hi2cout 0, (sdir1) 'write dir to the command register - decel to zero hi2cout [$B4], 3, (50) hi2cout 2, (0) hi2cout 0, (sdir2) switchoff 4 switchoff 5 switchoff 6 switchoff 7 'Flash warning LED on & off switchon 0 pause 250 switchoff 0 pause 250 switchon 0 pause 250 switchoff 0 'Go back and try another write-read loop goto MainLoop