{{ RoBo_Remote_v20.spin RoBo Remote - Multi Robot Remote Controller TheGrue John Harris http://letsmakerobots.com/node/18692 RoBo Remote - Check ID Pins and Acquire X and Y coordinates to send to remote robot }} CON _xinfreq = 5_000_000 ' 5 MHz cyrstal _clkmode = xtal1 + pll16x ' use crystal x 16 (sys clock = 80 MHz) '' ---Pin Assignments as indicated by the "p_" beginning --- '-- XBee Transceiver -- p_XBTx = 14 ' XBee Digital Input DIN p_XBRx = 15 ' XBee Digital Output DOUT '-- Accelerometer -- p_CLKPIN = 8 ' Accelerometer Clock Pin p_DATAPIN = 9 ' Accelerometer Data Pin p_CSPIN = 10 ' Accelerometer CS Pin '-- LED's -- p_Power = 0 ' LED p_Blue = 1 ' LED p_Green = 2 ' LED p_Yellow = 3 ' LED p_Red = 4 ' LED '-- XBee ID Switchs -- p_IdLow = 16 ' XBee ID Slider Switch p_IdMid = 17 ' XBee ID Slider Switch p_IdHigh = 18 ' XBee ID Slider Switch '-- Slide Switches -- p_SldRight = 19 ' Right Slider Switch p_SldMiddle = 20 ' Center Slider Switch p_SldLeft = 21 ' Left Slider Switch '-- Momentary Switches -- p_BlkLeft = 22 ' Left Black Momentary Switch p_BlkRight = 23 ' Right Black Momentary Switch p_RedLeft = 24 ' Left Red Momentary Switch p_RedRight = 25 ' Right Red Momentary Switch '' --- General Constants --- '-- XBee -- XB_Baud = 9600 ' XBee Baud Rate MY_Addr = 0 ' XBee MY Address of the RoBo Remote '-- Accelerometer -- X_OFFSETVAL = 0 ' X-Axis offset compensation value Y_OFFSETVAL = 55 ' Y-Axis offset compensation value Z_OFFSETVAL = 0 ' Z-Axis offset compensation value X_Adjust = -14 Y_Adjust = 0 Z_Adjust = -4 OBJ XB: "XBee_Object" 'XBee Communication Object SPI: "MMA7455L_SPI_v2" 'Used to Communicate to the Accelerometer vp: "Conduit" 'Viewport Debugger Object VAR long XData, YData, ZData, X, Y, Z, Forward, Turn, dataIn, DL_Addr, OverRide, Speed, Reverse, FollowMe, EngageRC, LeftBlack, LeftRed byte Stack[100] long Stack2[20] PUB Main | Count ' --- Set Pins as Inputs or Outputs --- dira[p_Power..p_Red]~~ dira[p_IdLow..p_RedRight]~ dira[p_XBTx]~~ dira[p_XBRx]~ '-------------- Initialization -------------- outa[p_Power]~~ ' Ligh Red Power Switch LED ' Diagnostics ' Set ViewPort up to connect for Diagnostics - comment out when not used '-- LED Diagnosic Pattern -- Repeat 4 Repeat Count from 4 to 1 Outa[Count]~~ !outa[Count + 1] !outa[p_Power] waitcnt(clkfreq/12 + cnt) Repeat Count from 1 to 4 Outa[Count]~~ !outa[Count - 1] !outa[p_Power] waitcnt(clkfreq/12 + cnt) Outa[p_Red..p_Blue]~ '-- Initialize Accelerometer and variables -- InitAccelerometer(p_CLKPIN, p_DATAPIN, p_CSPIN) '-- Configure XBee -- DL_Addr := ina[p_IdHigh..p_IdLow] ' Read the RoBo Remote ID pins and assign them to DL_Adder XB.start(p_XBRx, p_XBTx, 0, XB_Baud) ' Initialize comms for XBee XB.AT_Init ' Fast AT updates XB.AT_ConfigVal(string("ATMY"), MY_Addr) ' Assign RoBo Remote's ID address XB.AT_ConfigVal(string("ATDL"), DL_Addr) ' Assign Destination ID address ' cognew(acceptData,@Stack) ' start cog to accept incoming data '-------------- Main Routine -------------- Cognew(ReadButtons, @Stack2) SendControl PUB InitAccelerometer(CLKPIN, DATAPIN, CSPIN) 'Initialize the device SPI.start(CLKPIN, DATAPIN, CSPIN) 'Starts a Cog and sends pin information of the Accelerometer waitcnt(clkfreq/100+cnt) 'Give some time for communications to stabilize 'Write the X, Y, and Z offset compensation values SPI.write(SPI#XOFFL, X_OFFSETVAL) SPI.write(SPI#XOFFH, X_OFFSETVAL >> 8) SPI.write(SPI#YOFFL, Y_OFFSETVAL) SPI.write(SPI#YOFFH, Y_OFFSETVAL >> 8) SPI.write(SPI#ZOFFL, Z_OFFSETVAL) SPI.write(SPI#ZOFFH, Z_OFFSETVAL >> 8) Pub SendControl repeat if DL_Addr <> ina[p_IdHigh..p_IdLow] ' If ID switches have been changed ChangeTransmitAddress ' Change the Destination ID address elseif LeftBlack == 1 XB.str(string("t")) ' ' send "t" for TOBI! XB.delay(500) elseif FollowMe == 1 outa[p_Yellow]~~ XB.str(string("f")) ' ' send "f" for Follow Me Mode XB.delay(500) outa[p_Yellow]~ elseif LeftRed == 1 ' if Right Slider Switch off outa[p_Red]~~ XB.str(string("h")) ' ' send "h" to Halt the robot XB.delay(500) outa[p_Red]~ elseif EngageRC == 0 ' if Right Slider Switch off outa[p_Green]~~ XB.str(string("h")) ' ' send "h" to Halt the robot XB.delay(500) outa[p_Green]~ elseif EngageRC == 1 ' if Right Slider Switch on ' Read Accelerometer for Left and Right pulses ReadAccData ' Check for overide button if OverRide == 1 outa[p_Red]~~ ' send drive data(d) to bot outa[p_Blue]~~ XB.tx("d") XB.DEC(Forward) XB.tx(13) XB.DEC(Turn) XB.tx(13) XB.DEC(Speed) XB.tx(13) XB.DEC(OverRide) XB.tx(13) XB.DEC(Reverse) XB.tx(13) outa[p_Blue]~ outa[p_Red]~ XB.delay(100) Pub AcceptData '' accepts incoming data from bot to light Demo board LED's '' based on PING distance ' dira[16..23]~~ ' set LEDs as outputs ' repeat ' DataIn := XB.Rx ' read incoming data ' if DataIn == "u" or DataIn == "m" ' if update (u)or mapping (m)... ' Range := XB.RxDecTime(100) ' get range ' if Range < 800 ' if less the 0.8 meters (1 LED per 0.1m or 100mm) ' Light LEDs by shifting 1's by scaled range ' outa[16..23] := %11111111 << 8 >> (8 - range/100) ' else ' if > 0.8m, turn off LEDs ' outa[16..23]~ ' XB.rxFlush PUB ReadAccData SPI.write(SPI#MCTL, (%0110 << 4)|(SPI#G_RANGE_8g << 2)|SPI#G_MODE) 'Initialize the Mode Control register XData := SPI.read(SPI#XOUT8) 'Raw data for X-axis YData := SPI.read(SPI#YOUT8) 'repeat for Y-axis ZData := SPI.read(SPI#ZOUT8) 'and Z-axis ' Adjust Raw readings to 0 X := (~XData + X_Adjust) 'Take XYZData and add the XYZAdjust to make 0 and use ~ for Y := (~YData + Y_Adjust) 'making XYZData + or - numbers instead of 0-1=254 you get Z := (~ZData + Z_Adjust) '0-1=-1 ' Find common forward speed from X and Z-axis if Reverse == 0 if Z =< -1 'If Z is a Negative number Then convert to Pulse width by taking the Neg X number, Forward := X 'multiply by 50 add the result to 1500 center pulse and limit the minimum value to 1050 else 'If Z is a Positive number Then convert by taking the Absolue Value (||)of the Neg X number, Forward := ||X 'multiply by 50 add the result to 1500 center pulse and limit the maximum value to 2050 else if Z =< -1 'If Z is a Negative number Then convert to Pulse width by taking the Neg X number, Forward := ||X 'multiply by 50 add the result to 1500 center pulse and limit the minimum value to 1050 else 'If Z is a Positive number Then convert by taking the Absolue Value (||)of the Neg X number, Forward := X 'multiply by 50 add the result to 1500 center pulse and limit the maximum value to 2050 ' Pad the Forward to stop the gitters in the center 3 postitons If Forward =< 2 AND Forward => -2 Forward := Forward := 0 ElseIf Forward =< -3 Forward := Forward + 2 ElseIf Forward => 3 Forward := Forward - 2 ' Figure out Turn from Y-axis Turn := Y ' Pad the Turn to stop the gitters in the center 3 positions If Turn =< 2 AND Turn => -2 Turn := Turn := 0 ElseIf Turn =< -3 Turn := Turn + 2 ElseIf Turn => 3 Turn := Turn - 2 PUB ReadButtons Repeat EngageRC := ina[p_SldRight] Speed := ina[p_SldMiddle] Reverse := ina[p_SldLeft] OverRide := ina[p_RedRight] If ina[p_BlkLeft] == 1 if LeftBlack == 0 LeftBlack := 1 else LeftBlack := 0 Repeat until ina[p_BlkLeft] == 0 waitcnt(clkfreq/10 + cnt) If ina[p_RedLeft] == 1 if LeftRed == 0 LeftRed := 1 else LeftRed := 0 Repeat until ina[p_RedLeft] == 0 waitcnt(clkfreq/10 + cnt) If ina[p_BlkRight] == 1 if FollowMe == 0 FollowMe := 1 else FollowMe := 0 Repeat until ina[p_BlkRight] == 0 waitcnt(clkfreq/10 + cnt) PUB ChangeTransmitAddress outa[p_Red]~~ ' Turn on Red LED during the reprogramming DL_Addr := ina[p_IdHigh..p_IdLow] ' Read the RoBo Remote ID pins and assign them to DL_Adder XB.AT_Init ' Fast AT updates XB.AT_ConfigVal(string("ATDL"), DL_Addr) ' Assign Destination ID address outa[p_Red]~ ' Turn off the LED after the XBee is reprogrammed PUB Diagnostics vp.config(string("start:terminal")) vp.config(string("var:Forward, Turn, dataIn, DL_Addr, OverRide, Speed, Reverse, FollowMe, EngageRC, LeftBlack, LeftRed")) vp.config(string("text:timescale=10ms")) vp.share(@Forward,@LeftRed)