PINNED - Roboteq Controller - developing for powerchairs

[Burgerman]

Will try
throttle = getvalue(_CIP, 1) ' Read the Analog command before it is applied to the motor
steering = getvalue(_CIP, 2) ' Same for second

if throttle > 0 then throttle = (throttle * 100 ) / 100 ' Make positive power 100% of input.
steering = (steering * 35 ) / 100 ' make steering 35% of input

Else throttle = (throttle * 25 ) / 100 ' Make negative power 25% of input.
steering = (steering * 30) / 100 ' make steering 30% of input

setcommand (_G, 1, throttle) ' apply new throttle value
setcommand (_G, 2, steering) ' apply new steering

end if

wait (10)
goto top ' repeat every 10ms



Tommorow as I am thirsty. Will get there in the end!

[woodygb]

What script are you running now?

Minor mistake in the comment ...Read the Analog command before it is applied to the motor .... should be ....Read the ------ command before it is applied to the motor

[Burgerman]

Cosmas plus the added end if that was missing. But it does strange stuff!

[LROBBINS]

For both Woody and John,

I think I'll answer the questions about what I wrote to John in the message I will post tomorrow that is mostly addressed to Woody; but I'll just give a couple hints here. _CxI 1 and _CxI 2 are the command values AFTER all internal calculations, including mixing. So we can't think of them as Speed and Turn, but have to think about them as LeftMotorCommand and RightMotorCommand. Yes, Woody, you are right CPI, CSI and CAI are equivalents for pulse, serial or analog. More, much more, tomorrow, mostly written but I'm far too tired now to continue.

John,

Congratulations on getting some code to compile; that means you have a good idea how to use the interface and interpret its messages. Now you really do have to learn something about putting together a program, and it is something I think one has to learn piece by piece not all in one fell swoop. I don't think you'd want someone to try to build a BM2 before he could make a reasonably straight cut with a hack saw. Or would you?

Ciao,
Lenny

[woodygb]

top:
throttle = getvalue(_CIA, 1) ' Read the Analog command before it is applied to the motor
steering = getvalue(_CIA, 2) ' Same for second

if throttle < 0 then throttle = (throttle * 70) / 100 ' Make negative power 70% of input. Use ((n * 70) / 100) instead of n * 0.7
if steering = (steering * 65) / 100 ' make steering 65% of input

setcommand (_G, 1, throttle) ' apply new throttle value
setcommand (_G, 2, steering) ' apply new steering

end if

wait (10)
goto top ' repeat every 10ms


I don't think the
if steering = (steering * 65) / 100 ' make steering 65% of input
is correct ....as you saying IF without posing a question
Try ...
steering = (steering * 65) / 100 ' make steering 65% of input

So using _ CIP instead of _CIA
top:
throttle = getvalue(_CIP, 1) ' Read the .... command before it is applied to the motor
steering = getvalue(_CIP, 2) ' Same for second

if throttle < 0 then throttle = (throttle * 70) / 100 ' Make negative power 70% of input. Use ((n * 70) / 100) instead of n * 0.7
steering = (steering * 65) / 100 ' make steering 65% of input

setcommand (_G, 1, throttle) ' apply new throttle value
setcommand (_G, 2, steering) ' apply new steering

end if
wait (10)

[woodygb]

The other alternative is to add in the question to the steering....
NOTE this script is just for the one direction ...BUT once it works it should be easy to add the other.

top:
throttle = getvalue(_CIP, 1) ' Read the .... command before it is applied to the motor
steering = getvalue(_CIP, 2) ' Same for second

if throttle < 0 then throttle = (throttle * 70) / 100 ' Make negative power 70% of input. Use ((n * 70) / 100) instead of n * 0.7
if throttle < 0 then steering = (steering * 65) / 100 ' make steering 65% of input

setcommand (_G, 1, throttle) ' apply new throttle value
setcommand (_G, 2, steering) ' apply new steering

end if
wait (10 )

[Burgerman]

Thanks. Will test all tomorrow and report back. The thing where it sort of doesent go to pulse mode and sits in serial mode with intermittent control worries me. It does not do this with no script. It just goes to pulse input as soon as the reciever is powered up.

[Fulliautomatix]

The thing where it sort of doesent go to pulse mode and sits in serial mode with intermittent control worries me. It does not do this with no script. It just goes to pulse input as soon as the reciever is powered up.

'You' could probably put a line in to set the mode...

[woodygb]

Before Lenny posts a page and a half of instructions ..I'll throw in this.

To me this makes more sense than Comos's original of calling the throttle and using that term as is all the way thru the script.

throttle1 = original input value
steering1 = original input value
throttle2 = modified value
steering2 = modified value

The following should find the original values and modify them according to wether they are positive ( forward ) or negative ( reverse )

So the logic goes..

Start.

Read pulse number value .... GETVALUE

Ask the questions is this number greater or less than zero? IF

Take any answers that are YES/TRUE ... and modify the original input value accordingly. THEN

Take these modified values and use as output SETCOMMAND

End calcs END IF

Pause 10 m/secs & Rewind to Start WAIT ( 10 )


top:
throttle1 = getvalue(_CIP, 1) ' Read the pulse command before it is applied to the motor
steering1 = getvalue(_CIP, 2) ' Same for second

if throttle1 > 0 then throttle2 = (throttle1 * 70) / 100 ' Make forward speed 70% of input. Use ((n * 70) / 100) instead of n * 0.7
if throttle1 > 0 then steering2 = (steering1 * 65) / 100 ' make steering 65% of input

if throttle1 < 0 then throttle2 = (throttle1 * 30) / 100 ' Make reverse speed 30% of input.
if throttle1 < 0 then steering2 = (steering1 * 15) / 100 ' make steering 15% of input


setcommand (_G, 1, throttle2) ' apply new throttle value
setcommand (_G, 2, steering2) ' apply new steering

end if
wait (10 )

[LROBBINS]

It will be a while before I post my next long one, but I just want to repeat something so that people don't get off track.

THE _CPI or _CAI values are NOT throttle and turn! They are motor command values after ALL internal Roboteq computations, including mixing. If you use them as though they were throttle and turn, the chair IS NOT going to do what you want it to do.

CIao,
Lenny

[woodygb]

OK ... I'm picking the wrong point in the the command chain ...I've assumed ..erroniously it would seem ...that Cosmo's original script was correct...ish.

[woodygb]

So ..Lenny .. your saying that.
_CIA Channel Read Internal Analog Command
_CIP Channel Read Internal Pulse Command
_CIS Channel Read Internal Serial Command

Are seperate L.H /R.H MOTOR commands after the mix ... rather than one channel being Forward/Reverse on BOTH motors and the other channel a Steering modifier for BOTH motors?

Tsk ... does this mean having to write a mix for reduced steering?

[Burgerman]

I dont think that it can be correct. Because setting max reverse for cosma code to 70 percent, or 30 percent works. You get 100 forwards and 70 or 30 reverse. But turn does nothing.

But you can also set in the programming user interface, the same thing directly on the motors 1 and 2, and get the same result. So its possible to ignore the reverse speed part if need be. This is new in latest firmware.

[LROBBINS]

Woody,
That is what I meant, BUT ----

Burgerman,
If what you just said is correct, than what you saw in the little test you did the other day was misleading or mis-interpreted by me. I'll go ahead and post the rest of my "lecture", but let's keep in mind that if CIx are BEFORE mixing, I will have to change a lot of the calculation. Speed and Turn scaling would directly use CIx,1 and CIx,2 - actually simpler.

I will, however, have to redo the motor compensation and that's not going to be so easy. Motor Compensation has to be applied to each motor based on its individual current; that is on the actual motor signals AFTER mixing. To do that, I would have to calculate my own mix of CIx,1 and CIx,2, apply compensation, then un-mix to once again have values for Throttle and Turn signals, and finally send these to _G,1 and _G,2. And the Roboteq has two different mixing algorithms that have to be done and undone for these calculations! Grrr, I wish I had a Roboteq!

Ciao,
Lenny

[LROBBINS]

Had to leave for a while, so continuing from the last post.

If CIx are pre-mixing, doing motor compensation is going to be no mean task. I had elsewhere suggested the alternative of using reversed current sensors fed into a closed-loop mode (with D and I set to 0) and P proportional to motor resistance. I just realized that because of the integer arithmetic this may be impossible. P can take on values of 1, 2, etc., but motor resistance has values like 0.05 or 0.1 ohms. We could scale the sensors to a much smaller range than -1000 to 1000 to make using large values of P possible, but then the compensation will be in big stair steps (you'll see what I mean when I post the "lecture").

So, to do motor compensation I'd have to figure out how to find the individual motor values. If there are no built-in variables for this, that means mimicking the Roboteq mixing algorithm, compensating these mixed numbers, un-mixing them again, and using these for the new values to send to _G.

I'm glad to hear that Roboteq has now included reverse speed scaling in the firmware. Did they also implement turn scaling? Wouldn't it be nice if they implemented IR motor compensation too (for them it should be a snap; they surely have access to the post-mixing motor command values given that they calculate them somewhere).

So now, with the proviso that everything I assumed about the meaning of _CIx may be dead wrong, my next post will be the rest of what I've put together as a guide to writing Roboteq scripts (from someone who's never actually done one in his life, but has some programming experience).

Ciao,
Lenny

[woodygb]

Lenny,

If you go the whole hog and write a mix based on the X and Y joystick inputs you will end up with the two motor commands.


Lets do the simplest mix

RH motor command = Y value plus X value and LH motor command = Y value minus X value.. NOT ideal as it gives a diamond shape and puts dead

zone corners within the joysticks circle of movement.


OK the joystick output range is -1000 0 + 1000

With the X axis ( left and right on the stick ) centered and the Y axis at 1/2 way this will give 500 + 0 and 500 - 0
or 500 for each motor.

Can you not then apply torque compensation to either motor at this point? ..

[LROBBINS]

Before going back to my lecturing (once a professor always a professor), I'll point out some things I spotted in some of the posts. The lecture per se resumes at point (6).

(1) Woody, as for the multiplier, if you re-read what I wrote, I think you'll see that I said the same thing. One early version of the script (from someone at Roboteq no less) had (n*100)/70 when it SHOULD be (n*70)/100. I think it was gac3rd who first pointed this out.

(2) Both versions of the script that you (Woody) posted, with two IF constructs or one IF ELSE construct both have a little flaw. You've excluded 0 because you test for < 0 and for > 0. What will happen if the stick was displaced and is now moved back to center? The motor will continue running at its previous setting because 0 is never recognized to reset the motors to 0. So, one of the conditional (just one and it doesn't really matter which) must be changed to a <= or >= so centered-stick is recognized.

(3) A two-IF construct would work (instead of an IF ELSE) but each IF must have its own ENDIF, whereas with IF ELSE and IF ELSEIF you are chaining and nesting within one IF construct. Some professional programmers don't like ELSEIF constructs because it can be harder to spot problems like the excluded 0 in (2) above. Others like to always have an ELSE at the end with no conditional at all because that makes sure that all cases are covered.

Actually, however, neither the two IF nor the IF ELSE construct is necessary for what you're doing here AS LONG AS THE SetCommand lines come AFTER the ENDIF. Thus, if going backwards the values will be scaled and those will be sent to the motors. If not going backwards they will be unchanged. In either case they will be sent on to the motors. By now, John has surely gotten is ENDIF placement figure out.

(4) I think that some of this has already been covered in preceding posts. Pulse and serial inputs are scaled exactly the same way as analog, you just have to call _CIP or _CIS instead of _CIA. In a fancier version of the script, because the Roboteq can auto-switch from one to the other, you would want to first test for which mode is active and chose the _CIx according to which mode is being used. I don't see a anything in the command list that finds out which mode is active (have I just missed that?), so one might have to test with something like the following:

clip 1.jpg (65.23 KiB) Viewed 19906 times

(This stuff gets real hard to read without indenting. I'm therefore posting code as jpg images with a little formatting.)

(5) VERY IMPORTANT, BUT FROM LATER POSTINGS WHAT I'VE WRITTEN IN THIS PARAGRAPH MAY BE COMPLETELY WRONG!. What you've posted, and what I have written above assumes that _CIx,1 is Throttle and _CIx,2 is Turn. THEY ARE NOT (but maybe they are ???). This is not a MicroBasic thing, but a Roboteq thing. Recently Burgerman tested to see whether _CxI values are upstream or downstream of mixing. They are downstream - if he pushes the joystick straight forward, both :CIP 1 and CIP 2 go up indicating that CIP 1 is not speed - it is motor 1 - and CIP 2 is not turn - it is motor 2. This is great for doing motor compensation, which is why I asked him to run the test, but not so good for speed and turn scaling where we would want to modify Speed and Turn and not Motor 1 and Motor 2.

One can read the raw pot value before mixing, but there is no way I see to change those values and send them back into the Roboteq's calculation stream. That means that to scale turn we have to "decompose" the mixed _CIx values to find turning information. (If _Cix are actually pre-mixing, these calculations get much simpler, but doing motor compensation gets much more complex.)

For scaling speed, we have to find out (using both CIx values) whether we are going forwards or backwards (because we may want to apply different scaling for the two), and how fast we're going, and then scale that and apply the change equally to both motors. For turning, we have to apply half the scaling correction in one sense for one motor, and half in the other sense for the other motor. I wasn't too sure that I was thinking about this right, so I ran some examples in a spreadsheet before starting to convert it to MicroBasic.

Roboteq scaling.jpg (80.14 KiB) Viewed 19900 times

(6) Lecture resumes.

We now have an idea of what we want our program to do, so it's time to start writing it. While we're at it, however, let's try to structure the program a bit because we're probably going to want to add things and change things as time goes on.

I've put comments between {} braces, but that's not the way MicroBasic does comments - it uses a single quote mark to begin a comment, so don't just go copying stuff from here into a script. Creating a working script isn't my intent in any case; I'm just trying to share some ideas about things that might help write a good script. I don't have a Roboteq controller so I can't test a working script anyway, and I don't want to give you a runable, but untested, script.

First off, I wouldn't actually use the numbers 70 (speed scaling in reverse), 90 (speed scaling going forward) and 35 (turn scaling) where the calculations get done, but would specify some variables at the top of the script so that it would be easy to change them if they turn out not to be optimum.

clip 2.jpg (10.74 KiB) Viewed 19900 times

MicroBasic assumes all variables are Integer unless told otherwise. Because there are a few Boolean (TRUE or FALSE) variables I'm going to be wanting later on, I'll tell the program what they are here (although in MicroBasic I don't think you are required to do it at the beginning).

ON TO A NEW POST AS ONLY 3 ATTACHMENTS ARE ALLOWED PER POST

[LROBBINS]

clip 3.jpg (18.94 KiB) Viewed 19889 times

After this I'd put in any commands that I'd want to run just once at start up. Some examples: checking for a fused contactor, checking the brakes, turning on a light so I know the chair is live, anything else like that and any configuration items I'd rather handle from the script rather than from the GUI.

Next, because there may be multiple things we want to do over and over while driving, I will make a small main loop that calls some sub-routines that actually contain the "guts":

clip 4.jpg (23.94 KiB) Viewed 19888 times

This loop will keep going as long as the Roboteq is running and will keep telling the computer to do all those things. We might want to do other things as well. For example, for a front wheel drive chair, we might want turn rate to be scaled back or be limited as speed increases, and we could add a subroutine to do that. What actually has to get done is not, however, defined in this main loop, but in the subroutines. You could put all the guts in the main loop, but it would make the program hard to read and a "modular" organization makes it much easier to change or fix or add to later on. So now, let's see the subroutines (or at least a sketch of what they'd be like).

clip 5.jpg (85.95 KiB) Viewed 19891 times

[LROBBINS]

clip 6.jpg (147.98 KiB) Viewed 19884 times

Now we have to scale turn. Here I assume that we're going to scale turn to the same percentage whether going forward or backward, so there's no need for any IF. If we wanted different scaling for forward and reverse, this would be structured as in the ScaleSpeed subroutine and we'd need two scaling variables: e.g. vTurnScalingForward and vTurnScalingReverse. There is one critical difference in the calculations for turn scaling: we have to apply half of the turn correction in one direction on the left motor and in the opposite direction on the right.

clip 7.jpg (22.08 KiB) Viewed 19886 times

Now lets do IR motor compensation using some sensors attached to analog input 3 for the left motor and 4 for the right motor. Motor compensation is most useful at low torque, which is where the internal current estimate is least accurate. We've used the GUI program to configure these analog inputs to scale the -100 to 100 Amp sensors to the Roboteq's usual -1000 to 1000 range. I'd use sensors with a range a bit less than the maximum current our motors might use. Motor compensation is not really much use there, and by truncating we reduce the "twitchiness" that high compensation might cause when the chair is under extreme load.

We'll do all the calculations with everything in -1000 to 1000 Roboteq units. If you converted them back to -100 to 100 amps, the truncation of integer arithmetic would give gross steps in the amount of compensation. You'd have to convert back to the -1000 to 1000 range anyway to send the commands to the motor.

Here's a graph to illustrate what I've just said.

Motor Compensation.JPG (78.83 KiB) Viewed 19880 times

Compensation tapers off after 100 Amps and making the mistake of calculating it using Amps instead of Roboteq units gives a stair-step curve.

As I was doing this, I realized that there's absolutely no need for PID complexities for motor compensation. This is an open loop servo. - until the sensor is off scale, we're always compensating by the same percentage (the P in PID) and there's no time variation as we "approach" the right compensation (the I and D in PID). Thanks Woody for your old posts that pointed me in the right direction!

[LROBBINS]

Ok, now for the motor compensation subroutine:

clip 8.jpg (55.78 KiB) Viewed 19880 times

Finally, after making all of these adjustments we'll have a very short subroutine to actually send the new values to the motors.

clip 9.jpg (8.66 KiB) Viewed 19877 times

End of lecture. Now some miscellaneous "stuff".

Computers are STUPID. They don't know that tyre and tire are the same thing, and they won't recognize that "label" is a label unless you write "label:" with the colon - there'll be a compiler error that says something like "expected :". Typos are a bane of the programmer's existence and can be very hard to spot. Cut-and-paste is a friend for this, but, as Woody spotted in something I posted, it can be a false friend if you paste and forget to change a character that needs changing. Woody says I shouldn't feel bad about this. I won't - if I let myself feel bad about this I wouldn't get out of bed as it's something I do over and over and over again. The particular error Woody spotted, not changing a channel number from 1 to 2 if I recall, is particularly insidious. This will compile, and it will even run, but the program will do the wrong thing. It's going to take data from the wrong place, or send it to the wrong place, and the chair will not behave the way you expect it too. This kind of error, one that doesn't cause a compile or run-time error, but gives the wrong result is the hardest to spot and debug.

I guess I'm actually a Neanderthal when it comes to debugging. I started doing a bit of programming while in high school - almost 50 years ago. Modern GUI programming environments often have debugging tools that let you step through a program or follow variable values on screen and so on. I've never learned to use these as GUI debuggers, over the years, I'd learned to do this sort of stuff manually and still haven't joined the modern world. I don't think the MicroBasic editor has a debugger (I could be wrong), so debugging Roboteq scripts may have to be done the old way anyway. These "tricks of the trade" are really not much different from what we do when troubleshooting hardware:

clip 10.jpg (66.26 KiB) Viewed 19878 times

and so on.

Most BASIC versions don't care about upper case and lower case, ENDIF is the same as EndIf is the same as endif, so I try to write things so they're easy for me to read. Some compilers care a lot about case, for them vMyName is not the same as vMYNAME. For those languages it is essential to be consistent - for this, I use my cut-and-paste friend a lot, perhaps too much. I don't know how MicroBasic handles case, but John, and anyone else who actually has a Roboteq, will soon find out, or has already found out.

The computer has to know where every logical construct begins and ends or it would get completely confused. In C, one begins every logic block with a generic { and ends each one with }. In Pascal, different constructs begin with different keywords, but all end with the same END. In BASIC they're keyword pairs: for IF there's ENDIF, for WHILE there's ENDWHILE, for label: there's GoTo label and so on. If these starts and ends are not paired up, the compiler will tell you that you've screwed up, but the compiler error message may not help you much to find the cause. It will point to the line where the problem hits, not necessarily to the line that caused the error which can be some ways away from the line the compiler points to. The better the programmer understands what each of the logical constructs does, the easier it is to find the mistakes.

I don't know how much more I can do without actually having a Roboteq, but I think I'll download the software and see what it lets me do.

Ciao (and forgive me for my wordiness),
Lenny

[malamar]

we're always compensating by the same percentage (the P in PID) and there's no time variation as we "approach" the right compensation (the I and D in PID)

_ off course...not even thought of anything different...

Go ahead thinkin' squadron; at the end We`ll have a new, poweful, free to tinker, much needed controller

hip hip hip

[woodygb]

I ..a little bit like B.M. ..get terribly frustrated by script... not because I don't understand... but because the information I have available about the the order and form of the various commands is limited / full of holes ..like ..can I use two IF statements or shouild I ELSE the second or ....

The math is complex but fairly basic.

[Burgerman]

Wordiness is good here. Accuracy matters.

Not that I understood 90 percent of it but...

>>>I don't know how MicroBasic handles case, but John, and anyone else who actually has a Roboteq, will soon find out, or has already found out.

Doesent seem to matter. And Cosma from Roboteq seems to use all lower case.

You should download the software. It will answer lots of questions, has a testing option, and debugger etc built in. And a help button that explains a lot of stuff in great detail that I dont understand!

And I can test what you do. For what its worth you can test with a PC joystick if you have one, and watch what the inputs, mixing, outputs do on the graph etc. And you can run a hypothetical controller and not have one connected. Takes a bit to figure all this out unless I tell you in advance! Choose HDC2450 in "work offline" mode.

[LROBBINS]

Hi John,

I did download the software, and you're right - it's pretty good. I've only looked at the MicroBasic editor. I can write and compile a script. It gives reasonably good, but as I expected, rather hard to interpret compiler error messages. One reason for that is that the editor pane doesn't show line numbers, or at least I couldn't find a way to make it show them. So I either have to count, or manually add commented line numbers. You can also pull up a copy of the MicroBasic reference in your browser to try to help figure out what the errors were. Its color coding and automatic indent also helps -- it didn't take too long to discover that ENDIF should be END IF because it had the wrong color for a keyword, or that GetInput doesn't exist (it should have been GetValue) because it was the wrong color for a built-in Roboteq command.

It also let's you simulate running it, with inputs from the keyboard, so with some PRINT statements added you can see if the calculations and logic flow are what you intended.

Aside from lack of line numbers, there are a couple things I didn't like. There are no menus and hotkeys, so it's 100% mouse based - my bursitis (from a cervical disc problem) doesn't like this. The simulator output screen uses long form Roboteq command names, such as _CMDPLS whereas their examples and my script used the short form _CIP so one may need to have the table of these from the manual near at hand.

So a lot of the doubts about Roboteq and MicroBasic specific things that cause Woody to scratch his head are nicely resolved by just using this software even without having a Roboteq controller. Of course, you won't know how a chair will behave, and what calculation assumptions were wrong, until the program runs on real hardware.

I've added an attachment with my code such as it is. It has syntax errors and typos fixed so that it compiles and runs, and has some PRINT statements to show flow and how the values change. I changed the suffix to zip so that I could attach it, but it's really an mbs (MicroBasic script) file, so it can just be loaded directly into the Roboteq software. BTW *.mbs is actually just a *.txt file.

Ciao,
Lenny

[woodygb]

Very nice ..thanks Lenny,
I'll have to see about borrowing a Roboteq ..as the one in the Robot died from abuse / shock loading....

[Burgerman]

Was just about to load it and test. But... File is not a valid archive error???

Zip opens and is empty?

[woodygb]

Open the "zip" with notepad ... then copy and paste ..it isn't a zipped file ..it's a text file..but

The extension txt is not allowed

...so to upload I believe Lenny called the extension ZIP.

[Burgerman]

Found this by FTP on my server...
Not sure I fully understand it...
At least the first part.

What exactly is it going to do? (Hopefully!)

And do I use it from the word mainloop? Inclusive?


DIM bPactive AS Boolean
DIM bSactive AS Boolean
DIM bAactive AS Boolean

vForwardScaling = 90
vReverseScaling = 70
vTurnScaling = 35
vMotorResistance = 100

MainLoop:
PRINT ("\n","START OF MAIN LOOP \n")
GoSub FindMode
GoSub ScaleSpeed
GoSub ScaleTurn
GoSub MotorCompensation
GoSub SetMotors
Wait (20)
GoTo MainLoop

FindMode:
vPleft = getvalue(_CIP,1)
vPright = getvalue (_CIP,2)
vSleft = getvalue(_CIS,1)
vSright = getvalue(_CIS,2)
vAleft = getvalue(_CIA,1)
vAright = getvalue (_CIA,2)
bPactive = TOBOOL (abs(vPleft)+abs(vPright) > 0)
bSactive = TOBOOL (abs(Sleft)+abs(Sright) > 0)
bAactive = TOBOOL (abs(Aleft)+abs(Aright) > 0)
IF bPactive THEN
vMode = 1
ELSEIF bSactive THEN
vMode = 2
ELSEIF bAactive THEN
vMode = 3
ELSE
vMode = 0
END IF
PRINT ("\n","Mode = ", vMode, "\n \n")
RETURN

ScaleSpeed:
IF vMode = 1 THEN
vLeft = GetValue(_CIP,1)
vRight = GetValue(_CIP,2)
ELSEIF vMode = 2 THEN
vLeft = GetValue (_CIS,1)
vRight = GetValue (_CIS,2)
ELSEIF vMode = 3 THEN
vLeft = GetValue (_CIA,1)
vRight = GetValue (_CIA,2)
ELSE
vLeft = 0
vRight = 0
END IF
vSpeed = ((vLeft + vRight)*10)/20
IF (vSpeed < 0) THEN
vScaledSpeed = (vSpeed*vReverseScaling)/100
vChangeSpeed = vSpeed - vScaledSpeed
ELSE
vScaledSpeed = (vSpeed*vForwardScaling)/100
vChangeSpeed = vSpeed - vScaledSpeed
END IF
vLeft = vLeft - vChangeSpeed
vRight = vRight - vChangeSpeed
PRINT ("\n" , "vL speed scaled = " , vLeft , " vR speed scaled = " , vRight, "\n \n")
RETURN

ScaleTurn:
vTurn = vLeft - vRight
vScaledTurn = (vTurn*vTurnScaling)/100
vChangeTurn = vTurn - vScaledTurn
vLeft = vLeft - vChangeTurn/2 'subtracted
vRight = vRight + vChangeTurn/2 'added
PRINT ("vL turn scaled = " , vLeft , " vR turn scaled = " , vRight, "\n \n")
RETURN

MotorCompensation:
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
vLeft = vLeft + (vILeft*vMotorResistance)/1000
vRight = vRight + (vIRight*vMotorResistance)/1000
PRINT ("\n" , "vL compensated = " , vLeft , " vR compensated = " , vRight, "\n \n")
RETURN

SetMotors:
SetCommand (_G, 1, vLeft)
SetCommand (_G, 2, vRight)
PRINT ("\n" , "motor commands set \n \n")
RETURN

[woodygb]

I simply copied and pasted it ALL into Roborun ..it seemed happy with it.

[woodygb]

MainLoop:
GoSub FindMode
GoSub ScaleSpeed
GoSub ScaleTurn
GoSub MotorCompensation
GoSub SetMotors
Wait (20)
GoTo MainLoop

This says runs thru the 5 parts of the program in order .. GoSub = Jump to this part ..

GoTo Mainloop... directs it back to the beginning to do it all again.