'p8 - red led 'p9 - TO WC-132 Comm RXD, PIN 1 'p10 - rcv - to WC-132 Comm TXD, pin 3 '{$STAMP BS2} 'STAMP directive (specifies a BS2E) ' {$PBASIC 2.5} ' {$PORT COM5} velocity VAR Word rotation VAR Word distance VAR Word(2) angle VAR Word voltage VAR Word channel VAR Byte digital VAR Byte difference VAR Word sum VAR Word backup VAR Bit status VAR Word angmode VAR status.BIT0 velmode VAR status.BIT1 dstmode VAR status.BIT2 rotmode VAR status.BIT3 done VAR status.BIT4 running VAR status.BIT5 i2cmode VAR status.BIT6 rs232mode VAR status.BIT7 moten VAR status.BIT8 cmdoverflow VAR status.BIT9 portoverflow VAR status.BIT10 delayedcoast VAR status.BIT11 ldir VAR status.BIT12 lclk VAR status.BIT13 rdir VAR status.BIT14 rclk VAR status.BIT15 recBytes VAR Byte(4) ' define which pins connect to which signals RedLed PIN 8 Xmit PIN 9 Rcv PIN 10 '#DEFINE NORMAL_COMM = 1 DEBUG "starting up",CR OUTPUT RedLed GOTO main getack: SERIN Rcv, 84, [STR recBytes\2] IF recBytes(0) <> "a" THEN DEBUG "cmd failed to get ack:", recBytes(0), CR ENDIF RETURN Sync: DO RedLed = 1 sendit: SEROUT Xmit, 84, ["."] ' 6 = 38400, 84 = 9600, 396 = 2400 RedLed = 0 SERIN Rcv, 84, 100, sendit, [STR recBytes\2] RedLed = 1 IF recBytes(0) = "." THEN godone DEBUG "bad char: ", HEX recBytes(0), "H '", recBytes(0), "'",CR LOOP godone: RETURN getname: RedLed = 1 SEROUT Xmit, 84, ["N", LF] ' Send the name request SERIN Rcv, 84, [STR recBytes\4] 'DEBUG "Device Name = ", recBytes(0), recBytes(1), recBytes(2), recBytes(3) SERIN Rcv, 84, [STR recBytes\2] recBytes(2) = 0 'DEBUG recBytes(0), recBytes(1), CR RedLed = 0 recBytes(3) = 0 RETURN getstatus: 'GOSUB sync SEROUT Xmit, 84, ["S", LF] SERIN Rcv, 84, [WAIT("S"), HEX4 status, STR recBytes\1] 'DEBUG "Status = ", HEX4 status, CR RETURN waitdone: angle = 0 waitdone2: GOSUB getstatus IF done THEN DEBUG "motion complete ", CR IF done = 1 THEN RETURN PAUSE 20 IF angle = status THEN waitdone2 ' IF velmode THEN DEBUG "vel " ' IF dstmode THEN DEBUG "dst " ' IF delayedcoast THEN DEBUG "cst " ' IF running THEN DEBUG "run " ' DEBUG "s=", HEX4 status, CR angle = status GOTO waitdone2 RETURN setwaitmode: SEROUT Xmit, 84, ["F028B", LF] ' mode = counter rotating axes, inverted platform polarity, notify, slow comms GOSUB getack RETURN setnowaitmode: SEROUT Xmit, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms GOSUB getack RETURN coast: SEROUT Xmit, 84, ["C", LF] GOSUB getack RETURN reset: SEROUT Xmit, 84, ["R", LF] GOSUB getack RETURN setvelocity: 'DEBUG "entering set velocity = ", DEC velocity, CR SEROUT Xmit, 84, ["V", HEX4 velocity, LF] 'DEBUG "sent command; waiting for status", CR GOSUB getack RETURN getvelocity: SEROUT Xmit, 84, ["V", LF] SERIN Rcv, 84, [WAIT("V"), HEX4 velocity, STR recBytes\1] 'DEBUG "velocity = ", DEC velocity, CR RETURN setacceleration: 'DEBUG "entering set acceleration = ", DEC acceleration, CR SEROUT Xmit, 84, ["A", HEX4 velocity, LF] 'DEBUG "sent command; waiting for status", CR GOSUB getack RETURN getacceleration: SEROUT Xmit, 84, ["A", LF] SERIN Rcv, 84, [WAIT("A"), HEX4 velocity, STR recBytes\1] 'DEBUG "acceleration = ", DEC acceleration, CR RETURN setrotation: SEROUT Xmit, 84, ["Y", HEX4 rotation, LF] GOSUB getack RETURN getrotation: SEROUT Xmit, 84, ["Y", LF] SERIN Rcv, 84, [WAIT("Y"), HEX4 rotation, STR recBytes\1] 'DEBUG "rotation = ", DEC rotation, CR RETURN setdistance: SEROUT Xmit, 84, ["X", HEX4 distance(1), HEX4 distance(0), LF] GOSUB getack DEBUG "set distance = ", DEC distance(1), DEC distance(0), CR RETURN getdistance: SEROUT Xmit, 84, ["D", LF] SERIN Rcv, 84, [WAIT("D"), HEX4 distance(1), HEX4 distance(0), STR recBytes\1] DEBUG "get distance = ", DEC distance(1), DEC distance(0), CR RETURN setangle: SEROUT Xmit, 84, ["W", HEX4 angle, LF] GOSUB getack DEBUG "set angle = ", DEC angle, CR RETURN getangle: SEROUT Xmit, 84, ["W", LF] SERIN Rcv, 84, [WAIT("W"), HEX4 angle, STR recBytes\1] DEBUG "get angle = ", DEC angle, CR RETURN go: SEROUT Xmit, 84, ["G", LF] GOSUB getack RETURN getservovoltage: SEROUT Xmit, 84, ["H07", LF] SERIN Rcv, 84, [WAIT("H07"), HEX4 voltage, STR recBytes\1] voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51) DEBUG "servo voltage = ", DEC voltage, CR RETURN getlogicvoltage: SEROUT Xmit, 84, ["H06", LF] SERIN Rcv, 84, [WAIT("H06"), HEX4 voltage, STR recBytes\1] voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51) DEBUG "logic voltage = ", DEC voltage, CR RETURN getanalog: SEROUT Xmit, 84, ["H", HEX2 channel, LF] SERIN Rcv, 84, [WAIT("H"), HEX2 channel, HEX4 voltage, STR recBytes\1] ' DEBUG "channel ", DEC channel, " voltage = ", DEC voltage, CR RETURN setdigital: SEROUT Xmit, 84, ["J", HEX1 channel, HEX1 digital, LF] GOSUB getack 'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR RETURN getdigital: SEROUT Xmit, 84, ["J", HEX2 channel, LF] SERIN Rcv, 84, [WAIT("J"), HEX2 channel, HEX2 digital, STR recBytes\1] 'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR RETURN initfw: SEROUT Xmit, 6, ["F0302", LF] ' at 38400, set baud = 9600 GOSUB getack SEROUT Xmit, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms GOSUB getack RETURN Main: 'GOSUB initfw GOSUB sync DEBUG "WheelCommander Synchronized", CR GOSUB getname DEBUG "Clearing Notify Mode", CR GOSUB setnowaitmode DEBUG "Coast", CR GOSUB coast DEBUG "Reset", CR GOSUB reset DEBUG "Get Status", CR GOSUB getstatus DEBUG "Get Servo Voltage", CR GOSUB getservovoltage DEBUG "Get Logic Voltage", CR GOSUB getlogicvoltage DEBUG "Digital pins: ", CR FOR channel = 0 TO 3 GOSUB getdigital DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR NEXT velocity = 140 GOSUB setacceleration velocity = 70 GOSUB setvelocity Loop4: GOSUB getdistance RedLed = 1 angle = 0 GOSUB setangle distance(0) = 120 distance(1) = 0 GOSUB setdistance GOSUB go GOSUB waitdone ' GOSUB coast GOSUB getdistance RedLed = 0 angle = 90 GOSUB setangle distance(0) = 0 distance(1) = 0 GOSUB setdistance GOSUB go GOSUB waitdone ' GOSUB coast GOTO LOOP4 END