|
Servo Programming - 1 |
|
<= Back to Co-Processor page Here is one traditional way to drive 2 servos with a Basic Stamp. PBASIC code for that segment could be - |
| Servo Controller Subroutine | |||
| run_servos: | |||
| FOR I = 1 TO 10 | 'Will drive servos for about 200ms | ||
| PULSOUT LEFT,750 | 'pulses 1.5 ms to center LEFT servo | ||
| PULSOUT RIGHT,750 | 'now RIGHT one | ||
| PAUSE 17 | 'finish out 20 ms for servo | ||
| NEXT | 'keep looping for 10*20 = 200ms | ||
| RETURN | |||
|
This works but - |
| 1. |
It takes the whole processing power of the Stamp processor.
While the PULSOUT and PAUSE instructions are executing, the Stamp is incapable
of doing any other processing. |
| 2. | If you have to do other things, you can put them before the PAUSE. You may have to compensate for the extra code in the PAUSE length though. As long as it takes less than 17 ms and is relatively consistent, you can correct for it. |
| 3. | Depending on the added code length you may need to put a scope on the servo outputs to make sure of the timing when you are done programming and again after any program changes. |
|
You can use a Finite State Machine approach as in the following code - |
| FSM based servo controller subroutine for a robot | ||
| Main: | ||
| GOSUB action1 | 'do random wander about subroutine | |
| GOSUB action2 | 'do the IRPD subroutine | |
| GOSUB action3 | 'do the bumper subroutine | |
| GOSUB run_servos | 'run the servos for this pass | |
| GOTO main | 'keep looping doing this stuff | |
| run_servos: | ||
| IF aDur > 0 THEN aDec | 'Skip this pass if not 6th time through | |
| aDur = 6 | 'Reset cycle counter | |
| ' (compensated for extra decr) | ||
| PULSOUT LEFT,750 | 'pulses 1.5 ms to center LEFT servo | |
| PULSOUT RIGHT,750 | 'now RIGHT one | |
| aDec: | ||
| aDur = aDur - 1 | ||
| RETURN | ||
|
This also works but - |
| 1. | It still takes the whole processing power of the Stamp processor while the PULSOUT and PAUSE instructions are executing. |
| 2. | You do the other robotic things in the subroutines called by the GOSUBs. |
| 3. | You still have to compensate for the extra code in the GOSUBs though. aDur = 6 ASSUMES that all the processing in all the GOSUBs consistently take 2.8ms (= 17/6ms) to execute. That is not a lot of time! In fairness, as long as they take less than 17 ms and are relatively consistent, you can still correct for the execution time by fiddling with aDur's value. |
| 4. | Unless you are doing nothing much in the GOSUBs, you will probably STILL need to put a scope on the servo outputs to make sure of the timing when you are done programming and again after any program changes. |
|
Now let's try it again using two of the eight servo controllers. Our Libby robot's servo motors are on channels 0 and 1 so let's use them. In the default or "fine" mode, our servo controller breaks the 1 millisecond to 2 millisecond servo pulse widths into 256 steps of 4 microseconds each. 0 is approximately 1 ms and 255 = 2 ms. A value of 127 will center an unmodified servo or stop a modified one. |
| ServVal_0 | VAR BYTE | 'to load servo 0 position |
| ServVal_0_Old | VAR BYTE | 'remember old servo 0 position |
| ServVal_1 | VAR BYTE | 'to load servo 1 position |
| ServVal_1_Old | VAR BYTE | 'remember old servo 1 position |
| Initialize the program | |||
| ServVal_0 = 127 | |||
| ServVal_0_Old = 0 | |||
| ServVal_1 = 127 | |||
| ServVal_1_Old = 0 | |||
| AUXIO | '2p40 only | ||
| SEROUT 10, 1021, [116] | |||
| SEROUT 10, 1021, [116] | 'Reset Co-Processor | ||
| 'Reset turns servos off | |||
| Main: | |||
| GOSUB action1 | 'do the random wander about | ||
| ' subroutine (not shown) | |||
| GOSUB action2 | 'do the IRPD subroutine (not shown) | ||
| GOSUB action3 | 'do the bumper subroutine (not shown) | ||
| GOSUB action4 | 'figure out servo values from results of | ||
| ' previous subroutines | |||
| IF (ServVal_0 = ServVal_0_Old) THEN DoServ1 | |||
| 'Only need to send servo data | |||
| ' if different from last time. | |||
| SEROUT 10, 1021, [136,ServVal_0] | 'Set Servo 0 = Left side | ||
| ServVal_0_Old = ServVal_0 | 'remember value sent | ||
| DoServ1: | |||
| IF (ServVal_1 = ServVal_1_Old) THEN DoneServ | |||
| 'Only need to send servo data | |||
| ' if different from last time. | |||
| SEROUT 10, 1021, [137,ServVal_1] | 'Set Servo 1 = Right side | ||
| ServVal_1_Old = ServVal_1 | 'remember value sent | ||
| DoneServ: | |||
| GOTO Main | 'keep looping doing this stuff | ||
|
What's different? |
| 1. |
The first example program sent servo pulses for 200 ms. Using servo
controllers allows the correct pulses to the servos by sending four bytes
to the Co-Processor - ONCE! - For 200ms or 2 days - it doesn't matter.
Change it when you are ready. |
| 2. | No further Stamp intervention is needed to keep sending the correct servo pulse values every 20 ms. |
| 3. | No need for a scope to check servo timing. Since it doesn't depend on your processor being there to send the pulses, your programming gets a LOT easier. |
| 4. | You now have almost the full power of your main processor back. |
| <= Back to Co-Processor page |