In a previous post I talked about how SniffBot was wired up to provide basic Arduino control over the four wheels, making it simple to write code made the car move around. However anyone who's coded robots knows that they have a habit of "doing their own thing" now and then, at which point you end up chasing them round the room, trying to grab them and find the off switch. One of the things I wanted to add was to an RC unit so I could take manual contral to bring it back home. It's also just great fun to drive the car around under RC control, and driving SniffBot around the hall was a popular activity at a recent STEMNet meeting.
Gettiing an RC system is a bit of an investment, but the good thing is that once you've got a transmitter you can buy additional receivers fairly cheaply. Planet T5 transmitters (packaged with a single receiver) cost around £65, but as I already had a couple from previous projects I just added an extra "R6m" receiver to make SniffBot RC-Ready.
RC Receivers like these encode the position of the joysticks in a format ready to plug directly into a Servo. Each channel has three pins: Ground, 5V(ish) and Signal. There's also a power-in connector, so that the receiver can distribute power to the servo's.
By happy coincidence the Motor Shield on the SniffBot has two headers for plugging in Servo's which use the same format. They're "intended" to be outputs, but they work just as well as inputs, so we can just connect the Shield and Receiver using a couple of 3way DuPont cables (these are horribly expensive from RC shops, but you can get them for pennies from China). These also provide power to the receiver. I plugged them into channels 2 and 3 which correspond to the right joystick (channel 1 is traditionally the "accelerator": left joystick front/back, though it depends how your transmitter is configured).
Servo's are controlled by a pulse that the receiver sends to them every 50th of a second or so. The position is controlled by the duration of that pulse, which should be in the range 1-2mS. We therefore need to wait for the pulse, start a timer, then wait for the pulse to end, and record the time.
make forwardBack digital input 10
make j1startTime number
make joystick1 number
when start
.forever
..wait until forwardBack
..set j1startTime to timer
..wait until not forwardBack
..set joystick1 to timer-j1startTime
..set joystick1 to ((joystick1*1000)-1.5)*2
..if joystick1 <-1
...set joystick1 to -1
..if joystick1 >1
...set joystick1 to 1
Having recorded the time in seconds, we convert to milliSeconds and take away the average value (1.5). We should now have a value between +/-0.5, so we double that to get a value between -1 and +1 which we can use in future calculations. There's also a couple of checks to make sure that the value we have is in range.
If we were doing this in Python or C we'd need to be make sure we don't "miss" a pulse while we're doing some other calculation, but in Sniff we just use the above code as-is, and let it run forever. It updates the value of joystick1 whenever theres a new pulse. To decode the second joystick, just duplicate the code, but reading from a input 9.
when start
.forever
..set leftDrive to joystick1
..set rightDrive to joystick2
..broadcast updateMotors
..wait 0.05 secs
Having got the position of the joysticks the simplest thing do to is to assign one joystick to each set of wheels, and do some driving. This is called tank steering, and works OK if you use the two forward/back sticks to control each side, but we can do better...
To make the car drive from the two channels of a single stick, with forward/back, left/right controls we need to calculate the power we should be sending to each set of wheels. In the RC world this is done with a V-Tail Mixer - a surprisingly common component, as its also used to control the ailerons of RC planes.
The maths behind v-tail mixing is at the same time totally trivial, and completely non-obvious:
when start
.forever
..set leftDrive to (joystick1+joystick2)
..if leftDrive>1
...set leftDrive to 1
..if leftDrive<-1
...set leftDrive to -1
..
..set rightDrive to (joystick1-joystick2)
..if rightDrive>1
...set rightDrive to 1
..if rightDrive<-1
...set rightDrive to -1
..broadcast updateMotors
..wait 0.05 secs
One side gets the sum of the two joysticks and the other gets the difference (its worth going through it yourself to figure out why this works, but it does). You may find your car drives backwards and/or steering is inverted when you try this. The Planet transmitter has a set of switches under the perspex panel which let you reverse the polarity of the joysticks, or you can fix it in software.
Here's the complete code:
#a fly-by-wire RC car...
######first read the RC input
######to get the two joystick values
######and a mode switch
make forwardBack digital input 10
make j1startTime number
make joystick1 number
when start
.forever
..wait until forwardBack
..set j1startTime to timer
..wait until not forwardBack
..set joystick1 to timer-j1startTime
..set joystick1 to ((joystick1*1000)-1.5)*2
..if joystick1 <-1
...set joystick1 to -1
..if joystick1 >1
...set joystick1 to 1
make leftRight digital input 9
make j2startTime number
make joystick2 number
when start
.forever
..wait until leftRight
..set j2startTime to timer
..wait until not leftRight
..set joystick2 to timer-j2startTime
..set joystick2 to ((joystick2*1000)-1.5)*2
..if joystick2 <-1
...set joystick2 to -1
..if joystick2 >1
...set joystick2 to 1
make modeSwitch digital input 2
make modeStartTime number
make manualMode boolean
when start
.forever
..wait until modeSwitch
..set modeStartTime to timer
..wait until not modeSwitch
..set manualMode to (timer-modeStartTime)>0.0015
##### NOW CONTROL THE MOTORS #####
make leftDrive number
make rightDrive number
make motorController device
make motor1 number
make motor2 number
make motor3 number
make motor4 number
when updateMotors
.set motor1 to leftDrive
.set motor2 to leftDrive
.set motor3 to rightDrive
.set motor4 to rightDrive
.tell motorController to "update"
##### USE THEINPUTS TO CALCULATE leftDrive and rightDrive
when start
.forever if manualMode
..set leftDrive to (joystick1+joystick2)
..if leftDrive>1
...set leftDrive to 1
..if leftDrive<-1
...set leftDrive to -1
..
..set rightDrive to (joystick1-joystick2)
..if rightDrive>1
...set rightDrive to 1
..if rightDrive<-1
...set rightDrive to -1
..broadcast updateMotors
..wait 0.05 secs
#What should go in here?
when start
.forever if not manualMode
..set leftDrive to 0
..set rightDrive to 0
..broadcast updateMotors
..wait 0.05 secs
This final version also adds a kill switch. The switch on the shoulder of the Planet transmitter transmits on channel 5, so I hooked that up in pin 3 - the one unused digital pin on the Motor Shield, and added code to make that control a boolean: manualMode. Now if we're in manual mode we use the joysticks to control the motors, but otherwise we set the motors to 0. Now we can halt our runaway robot with the flick of a switch.
In a future post I'll show add some smarter code in here so the car can execute tasks by itself (you could add line following code here), and when it decides to misbehave you can flip the switch back to manual and drive it back to base for repairs! No more chasing your robot round the room!
No comments:
Post a Comment