'p0 - red led 'p1 - xmit - to WC-116 Comm RXD, pin 1 'p2 - rcv - to WC-116 Comm TXD, pin 3 '{$STAMP BS2} 'STAMP directive (specifies a BS2E) ' {$PBASIC 2.5} ' {$PORT COM1} 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) RedLed PIN 0 Xmit PIN 1 Rcv PIN 2 '#DEFINE NORMAL_COMM = 1 DEBUG "starting up",CR OUTPUT RedLed GOTO main getack: SERIN 2, 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 1, 84, ["."] ' 6 = 38400, 84 = 9600, 396 = 2400 RedLed = 0 SERIN 2, 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 1, 84, ["N", LF] ' Send the name request SERIN 2, 84, [STR recBytes\4] 'DEBUG "Device Name = ", recBytes(0), recBytes(1), recBytes(2), recBytes(3) SERIN 2, 84, [STR recBytes\2] recBytes(2) = 0 'DEBUG recBytes(0), recBytes(1), CR RedLed = 0 recBytes(3) = 0 RETURN getstatus: 'GOSUB sync SEROUT 1, 84, ["S", LF] SERIN 2, 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 1, 84, ["F028B", LF] ' mode = counter rotating axes, inverted platform polarity, notify, slow comms GOSUB getack RETURN setnowaitmode: SEROUT 1, 84, ["F0289", LF] ' mode = counter rotating axes, inverted platform polarity, slow comms GOSUB getack RETURN coast: SEROUT 1, 84, ["C", LF] GOSUB getack RETURN reset: SEROUT 1, 84, ["R", LF] GOSUB getack RETURN setvelocity: 'DEBUG "entering set velocity = ", DEC velocity, CR SEROUT 1, 84, ["V", HEX4 velocity, LF] 'DEBUG "sent command; waiting for status", CR GOSUB getack RETURN getvelocity: SEROUT 1, 84, ["V", LF] SERIN 2, 84, [WAIT("V"), HEX4 velocity, STR recBytes\1] 'DEBUG "velocity = ", DEC velocity, CR RETURN setacceleration: 'DEBUG "entering set acceleration = ", DEC acceleration, CR SEROUT 1, 84, ["A", HEX4 velocity, LF] 'DEBUG "sent command; waiting for status", CR GOSUB getack RETURN getacceleration: SEROUT 1, 84, ["A", LF] SERIN 2, 84, [WAIT("A"), HEX4 velocity, STR recBytes\1] 'DEBUG "acceleration = ", DEC acceleration, CR RETURN setrotation: SEROUT 1, 84, ["Y", HEX4 rotation, LF] GOSUB getack RETURN getrotation: SEROUT 1, 84, ["Y", LF] SERIN 2, 84, [WAIT("Y"), HEX4 rotation, STR recBytes\1] 'DEBUG "rotation = ", DEC rotation, CR RETURN setdistance: SEROUT 1, 84, ["X", HEX4 distance(1), HEX4 distance(0), LF] GOSUB getack DEBUG "set distance = ", DEC distance(1), DEC distance(0), CR RETURN getdistance: SEROUT 1, 84, ["D", LF] SERIN 2, 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 1, 84, ["W", HEX4 angle, LF] GOSUB getack DEBUG "set angle = ", DEC angle, CR RETURN getangle: SEROUT 1, 84, ["W", LF] SERIN 2, 84, [WAIT("W"), HEX4 angle, STR recBytes\1] DEBUG "get angle = ", DEC angle, CR RETURN go: SEROUT 1, 84, ["G", LF] GOSUB getack RETURN getservovoltage: SEROUT 1, 84, ["H07", LF] SERIN 2, 84, [WAIT("H07"), HEX4 voltage, STR recBytes\1] voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51) DEBUG "servo voltage = ", DEC voltage, CR RETURN getlogicvoltage: SEROUT 1, 84, ["H06", LF] SERIN 2, 84, [WAIT("H06"), HEX4 voltage, STR recBytes\1] voltage = (voltage.HIGHBYTE * 5) + (voltage.LOWBYTE / 51) DEBUG "logic voltage = ", DEC voltage, CR RETURN getanalog: SEROUT 1, 84, ["H", HEX2 channel, LF] SERIN 2, 84, [WAIT("H"), HEX2 channel, HEX4 voltage, STR recBytes\1] ' DEBUG "channel ", DEC channel, " voltage = ", DEC voltage, CR RETURN setdigital: SEROUT 1, 84, ["J", HEX1 channel, HEX1 digital, LF] GOSUB getack 'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR RETURN getdigital: SEROUT 1, 84, ["J", HEX2 channel, LF] SERIN 2, 84, [WAIT("J"), HEX2 channel, HEX2 digital, STR recBytes\1] 'DEBUG "channel ", DEC channel, " digital = ", DEC digital, CR RETURN initfw: SEROUT 1, 6, ["F0302", LF] ' at 38400, set baud = 9600 GOSUB getack SEROUT 1, 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 7 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