PINNED - Roboteq Controller - developing for powerchairs

[LROBBINS]

Here are three scripts:

n. 1 is complete, but forces it to use only Mode 1. When simulating it will only ask you for two joystick input values and two AIC (current) values.

n. 2 does the calculations, but doesn't send anything on to the motors - they should just respond to the joystick as though the script weren't there.

n. 3 does no calculations, but sends the pulse joystick values directly on to the motors. Also for this one, motor outputs should just follow the joystick.

Download the files, change the suffix from zip to mbs, load into Roborun. Try BUILD. If they won't build, you've got a Roborun software problem. If they do BUILD, try simulate and see what the black lines say.

n. 1, for pulse inputs 300 300, AIC inputs 0 0 ---- 300 300 input values, 270 180 after scaling, and unchanged after motor compensation.

n. 2, for pulse inputs 300 300, AIC inputs 0 0 ---- same calculated values, but a note at the end saying "CALCULATIONS DONE BUT NOTHING SENT TO MOTORS"

n. 3, pulse inputs 300 300. Will not ask for AIC inputs as that's part of motor compensation. Note at end saying "UNCHANGED JOYSTICK VALUES SENT TO MOTORS"

Now hook up controller and try each of these. Do all of them cause the serial flibbity gibbity?

Ciao,
Lenny

[Burgerman]

An interesting thing.

If I comment out the line sub/motor compensation, it no longer goes to serial OR pulse when I run the script. The graph slows down slightly. The steering "works", as in I get one wheel back/other forwards by about 20 percent, ONLY with full forward speed stick pushed to 100 percent.. And neither Pulse OR serial input are indicated.

THats odd. And compensation causes it to go to serial mode.


Will now test new scripts.

[Burgerman]

script 1, 2 3

_CIP is not a valid named constant...

Dont get it. Why does it do this, how can I stop it? I have the latest everything...

[woodygb]

commands short and long.

command.jpg (145.04 KiB) Viewed 14851 times

[LROBBINS]

John,

Between having trouble recognizing _AIC (which are the current analog inputs used in the MotorCompensation subroutine), and the fact that commenting out that subroutine gets rid of the serial problem, I'm suspecting that a bug got introduced into the _AIC variable in the compiler.

You might try installing the Roborun that's shown as Download Roborun softward (or some such) instead of the one that came with the latest firmware update (I think that's what you said you have). It's RoborunPlusSetup-v12-072712.zip. If that's the same as the one you have installed, do you have an older version you could check? Re-BUILD with whatever Roborun version you decide to try. Does it still have the serial problem with motor compensation running?

An alternative is that there is some problem with what the controller is seeing, or not seeing, at analog input pins 3 and 4 which is what the _AIC are reading. I'll send you another script that reads the _AIC but then ignores them!

Ciao,
Lenny

[LROBBINS]

John,

Here's a script just like John 1 EXCEPT that, although it reads the analog values for motor compensation it then ignores them.

Ciao,
Lenny

[Burgerman]

Lenny the controller is disconnected for now.

The software is exactly the same as yours, and I just re downloaded, re installed, (and rebooted) and it still doesent like short names!

I have no analog input connected, never had, and its disabled and set to do nothing in settings. As are ALL other inputs other than RC.

Only PULSE /RC and SERIAL via PC USB.

When the COMPENSATION is disabled, it doesnt go to serial input. So something there is causing issues.

[Burgerman]

Its not name length, its the _ underscore it doesent like???

I removed it and its happy.

[LROBBINS]

John,

Removing the underscore made this into YOUR variable, something you use in the script to store a number, not a Roboteq named variable, something that has (or should have) a meaning in the firmware. AIC doesn't mean anything to the controller, _AIC should.

If _AIC is giving problems in BUILD then there's a problem in compiling it properly (the Roborun software). If it builds with _AIC, but using it gives the serial goofiness, then there's a problem in the firmware or in what it's seeing at serial input pins 3 and 4.

Do you have a copy of a previous firmware to see if it gives the same problem?

Ciao,
Lenny

[Burgerman]

number 1 = serial and one motor full power 1000 other 0 and tick box no affect.

number 2 = goes to pulse input, whoohoo! Full 100 percent turn/forwards/reverse But graph not smooth. It updates erratically.

number 3 = stuck in serial mode... not working

number 4 = works! restricted reverse, restricted turn, 90 percent forwards. But SAYS serial mode even when RC pulse, even muted, doesent say pulse, and graph extremely erratic.

[Burgerman]

And turn rate reduces in reverse?#

And in serial mode (unticked) driven by those sliders its really odd, spiky, not controllable.

And I had to remove 4 underscores. Yet sometimes its ok with them? Just put them back and now it builds? Damded computers!

But as it is its usable but graph update speed is affected severely? Maybe script too fast?

[LROBBINS]

Those underscores are ESSENTIAL to reading stuff from the controller. The underscore is what tells the computer that this is a controller variable. If you have to remove them to get the script to run, something is very wrong. The question now is whether it is just your software/firmware/setup, or whether there's a bug in the Roboteq compiler or firmware. Ciao, Lenny

[LROBBINS]

Oh, I forgot to say that turn rate should reduce more in reverse than in forward with the proportions set at the top of the script:

vForwardScaling = 90
vReverseScaling = 60
vTurnScalingFwd = 60
vTurnScalingRev = 25

Forward speed should be 80% of what it is without the script.

Reverse speed should be 60% of what it is without the script.

Turn rate when going forward is 60% of what it is without the script, and 25% in reverse.

Without comparing with and without script, the only things that are likely to be obvious is that reverse is slower than forward for the same stick displacement, and that reverse turn is slower than turn when going forward.

Ciao,
Lenny

[Burgerman]

Right 95, 30, 30, 30, fixes the steer and reverse speed and reverse turn rate speeds.

This is a 15mph chair, so my legs would fall off at 60 percent turn rate!

As it is it would work. Maybe need to look at why the graph goes slow intermittent when the script runs?

[LROBBINS]

John,

If you still have copies of the Roborun setup.zip that you used, and/or the setup.msi and setup.exe files, e-mail them to me and I will see if they have trouble building _AIC on my computer.

Ciao,
Lenny

[woodygb]

Would it be worth replacing ALL the shortened commands with the long?

[Burgerman]

They only do it inmtermittently here? I have several machines, all with the same version you have. For EG I just put the underscores back, and now it builds!???

Its also quite easy to set up "smooth" control responses. With a weak expo, some acceleration (delay of say .6 to full power second) and a small deadband. This responds much like my pilot plus does now.

[Burgerman]

Everything has underscores now and builds correctly. No idea why it sometimes faild.

Now, the analog inputs are set to disabled. They may "float" though? But should be tied by high value resistor to either high or low.

But 4 works.

[woodygb]

John..Are you using a Spektrum Radio?

[Burgerman]

JR, same thing. Yes with full linear movement to 100 percent.

[Burgerman]

Just retested all 4 now I can get them all to build. What gives?

1 and 4 both work fine. Although both STAY on serial, even when using the transmitter to control everything.

So I keep and use 1 for now?

[Burgerman]

Definitely looking better! Does compensation work? Only know by testing? Soon!

[LROBBINS]

The only difference between John 1 and John 4 is here:

John 1
MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)

John 4
MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)

vILeft = 0
vIRight = 0

Both read the values of analog input pins 3 and 4, but then in John 4 those values are ignored and 0 is used instead.

Rather than pulling pins 3 and 4 up or down, why not feed them from simple 0 to 5V voltage dividers. Then you'll be able to use John 1 and see how Motor Compensation works.

Hah, I see that you've already gotten John 1 to work while I was writing this. Now if you hook up some voltage dividers you can at least fake motor compensation. Once you've got that working, I can write another version in which you can chose whether to use external current sensors or the internal motor current estimates.

Maybe the lighting up of the "serial" on screen LED just reflects the fact that the script is running. Obviously, with John 1 you are reading ONLY pulse inputs because that's all it can do.

I can't imagine why you sometimes can and sometimes can't compile _AIC, and why some builds give weird responses, but your latest ones work. I guess I'd try doing things on another computer at this point! One that has no software running except the Roboteq. No browser, no wifi, no nothing.

Ciao,
Lenny

[Burgerman]

I did try the laptop. Same. But retry a few times and then it works... After that it keeps working until you rerstart the software and then it screws up again for a bit...

Either way I have it loadfed and can test.Not sure about the graph though. Gets all jerky when script runs. And serial doesent work... I expect analog to be no problem.

This means its at least usable and configurable without resistors etc.

What will it do to end point or out of band detection? Or the myriad of other options?

[LROBBINS]

What will it do to end point or out of band detection? Or the myriad of other options?

Should not affect them at all - what the script does is all downstream of that. _CIx is the value calculated after all of those things.

Ciao,
Lenny

[Burgerman]

And the graph issue is sorted out by setting wait to 50. Thats still fast. So all looking good.

Now, this version, as I have it here, in full in next post, is just PULSE?

So how do we change this to Pulse + Analog. So I get the same settings via chairs joystick or RC transmitter?

[Burgerman]

vForwardScaling = 95
vReverseScaling = 35
vTurnScalingFwd = 35
vTurnScalingRev = 35
vMotorResistance = 100

MainLoop:
PRINT ("\n","START OF MAIN LOOP \n")
' GoSub FindMode
GoSub ModeForcedTo1
GoSub SetCommands
PRINT ("\n" , "Joystick THROTTLE = ", vC1, " Joystick STEER = ", vC2, "\n")

'use following three lines if _CIx are M1 and M2 values
' GoSub ScaleSpeedM1M2
' GoSub ScaleTurnM1M2
' GoSub MotorCompensationM1M2

' use following three lines if _CIx are THROTTLE and STEER values
GoSub ScaleSpeedThrStr
GoSub ScaleTurnThrStr
GoSub MotorCompensationThrStr

GoSub SetMotors
Wait (50)
GoTo MainLoop

ModeForcedTo1:
vP1 = getvalue(_CIP,1)
vP2 = getvalue (_CIP,2)
' vS1 = getvalue(_CIS,1)
' vS2 = getvalue(_CIS,2)
' vA1 = getvalue(_CIA,1)
' vA2 = getvalue (_CIA,2)
' bPactive = TOBOOL (abs(vP1)+abs(vP2) > 0)
' bSactive = TOBOOL (abs(vS1)+abs(vS2) > 0)
' bAactive = TOBOOL (abs(vA1)+abs(vA2) > 0)
' IF bPactive THEN
' vMode = 1
' ELSEIF bSactive THEN
' vMode = 2
' ELSEIF bAactive THEN
' vMode = 3
' ELSE
' vMode = 0
' END IF
vMode = 1
PRINT ("\n","Mode = ", vMode, "\n \n")
RETURN

SetCommands:
IF vMode = 1 THEN
vC1 = vP1
vC2 = vP2
ELSEIF vMode = 2 THEN
vC1 = vS1
vC2 = vS2
ELSEIF vMode = 3 THEN
vC1 = vA1
vC2 = vA2
ELSE
vC1 = 0
vC2 = 0
END IF
RETURN

ScaleSpeedM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
IF (vSpeed < 0) THEN
vC1 = (vC1*vReverseScaling)/100
vC2 = (vC2*vReverseScaling)/100
ELSE
vC1 = (vC1*vForwardScaling)/100
vC2 = (vC2*vForwardScaling)/100
END IF
PRINT ("\n" , "M1 scaled speed = " , vC1 , " M2 scaled speed = " , vC2, "\n \n")
RETURN

ScaleSpeedThrStr:
vSpeed = vC1 'vC1=THROTTLE
IF (vSpeed < 0) THEN
vScaledSpeed = (vSpeed*vReverseScaling)/100
ELSE
vScaledSpeed = (vSpeed*vForwardScaling)/100
END IF
'set THROTTLE to new speed
vC1= vScaledSpeed
PRINT ("\n" , "THROTTLE/STEER scaled speed = " , vC1 , "\n \n")
RETURN

ScaleTurnM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
'unmix motor settings to find turn value that will be scaled
vTurn = vC1-vC2
IF (vSpeed < 0) THEN
vScaledTurn = (vTurn*vTurnScalingRev)/100
ELSE
vScaledTurn = (vTurn*vTurnScalingFwd)/100
END IF
'apply half of change to each wheel in opposite senses
vChangeTurn = vTurn - vScaledTurn
vC1 = vC1 - vChangeTurn/2 'subtracted
vC2 = vC2 + vChangeTurn/2 'added
PRINT ("M1 scaled turn = " , vC1 , " M2 scaled turn = " , vC2, "\n \n")
RETURN

ScaleTurnThrStr:
vSpeed = vC1 'vC1 = THROTTLE
vTurn = vC2 'vC2 = STEER
IF (vSpeed < 0) THEN
vC2 = (vTurn*vTurnScalingRev)/100
ELSE
vC2 = (vTurn*vTurnScalingFwd)/100
END IF
PRINT ("THROTTLE/STEER scaled turn = " , vC2 , "\n \n")
RETURN

MotorCompensationM1M2:
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'apply compensation to each motor
vC1 = vC1 + (vILeft*vMotorResistance)/1000
vC2 = vC2 + (vIRight*vMotorResistance)/1000
PRINT ("\n" , "vC1 compensated = " , vC1 , " vC2 compensated = " , vC2, "\n \n")
RETURN

MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'do mixing to find motor values (DO NOT TRUNCATE - we will be reversing this below)
vM1 = vC1 + vC2
vM2 = vC1 - vC2
'apply IR compensation
vM1 = vM1 + (vILeft*vMotorResistance)/1000
vM2 = vM2 + (vIRight*vMotorResistance)/1000
'undo mixing to get back new THROTTLE and STEER values
vC1 = (vM1 + vM2)/2
vC2 = (vM1 - vM2)/2
'truncate in case compensation pushed THROTTLE and/or STEER over limits
IF (vC1 > 1000) THEN
vC1 = 1000
END IF
IF (vC1 < -1000) THEN
vC1 = -1000
END IF
IF (vC2 > 1000) THEN
vC2 = 1000
END IF
IF (vC2 < -1000) THEN
vC2 = -1000
END IF
PRINT ("\n" , "compensated THROTTLE = " , vC1 , " compensated STEER = " , vC2, "\n \n")
RETURN

SetMotors:
SetCommand (_G, 1, vC1)
SetCommand (_G, 2, vC2)
PRINT ("\n" , "motor commands set to: THROTTLE = ", vC1, " STEER = ", vC2, "\n \n")
RETURN

[woodygb]

Remove some of the comment marks...and add back in the findmode.

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

vForwardScaling = 95
vReverseScaling = 35
vTurnScalingFwd = 35
vTurnScalingRev = 35
vMotorResistance = 100

MainLoop:
PRINT ("\n","START OF MAIN LOOP \n")
GoSub FindMode
GoSub SetCommands
PRINT ("\n" , "Joystick THROTTLE = ", vC1, " Joystick STEER = ", vC2, "\n")

'use following three lines if _CIx are M1 and M2 values
' GoSub ScaleSpeedM1M2
' GoSub ScaleTurnM1M2
' GoSub MotorCompensationM1M2

' use following three lines if _CIx are THROTTLE and STEER values
GoSub ScaleSpeedThrStr
GoSub ScaleTurnThrStr
GoSub MotorCompensationThrStr

GoSub SetMotors
Wait (50)
GoTo MainLoop

FindMode:
vP1 = getvalue(_CIP,1)
vP2 = getvalue (_CIP,2)
vS1 = getvalue(_CIS,1)
vS2 = getvalue(_CIS,2)
vA1 = getvalue(_CIA,1)
vA2 = getvalue (_CIA,2)
bPactive = TOBOOL (abs(vP1)+abs(vP2) > 0)
bSactive = TOBOOL (abs(vS1)+abs(vS2) > 0)
bAactive = TOBOOL (abs(vA1)+abs(vA2) > 0)
IF bPactive THEN
vMode = 1
ELSEIF bSactive THEN
vMode = 2
ELSEIF bAactive THEN
vMode = 3
ELSE
vMode = 0
END IF
vMode = 1
PRINT ("\n","Mode = ", vMode, "\n \n")
RETURN

SetCommands:
IF vMode = 1 THEN
vC1 = vP1
vC2 = vP2
ELSEIF vMode = 2 THEN
vC1 = vS1
vC2 = vS2
ELSEIF vMode = 3 THEN
vC1 = vA1
vC2 = vA2
ELSE
vC1 = 0
vC2 = 0
END IF
RETURN

ScaleSpeedM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
IF (vSpeed < 0) THEN
vC1 = (vC1*vReverseScaling)/100
vC2 = (vC2*vReverseScaling)/100
ELSE
vC1 = (vC1*vForwardScaling)/100
vC2 = (vC2*vForwardScaling)/100
END IF
PRINT ("\n" , "M1 scaled speed = " , vC1 , " M2 scaled speed = " , vC2, "\n \n")
RETURN

ScaleSpeedThrStr:
vSpeed = vC1 'vC1=THROTTLE
IF (vSpeed < 0) THEN
vScaledSpeed = (vSpeed*vReverseScaling)/100
ELSE
vScaledSpeed = (vSpeed*vForwardScaling)/100
END IF
'set THROTTLE to new speed
vC1= vScaledSpeed
PRINT ("\n" , "THROTTLE/STEER scaled speed = " , vC1 , "\n \n")
RETURN

ScaleTurnM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
'unmix motor settings to find turn value that will be scaled
vTurn = vC1-vC2
IF (vSpeed < 0) THEN
vScaledTurn = (vTurn*vTurnScalingRev)/100
ELSE
vScaledTurn = (vTurn*vTurnScalingFwd)/100
END IF
'apply half of change to each wheel in opposite senses
vChangeTurn = vTurn - vScaledTurn
vC1 = vC1 - vChangeTurn/2 'subtracted
vC2 = vC2 + vChangeTurn/2 'added
PRINT ("M1 scaled turn = " , vC1 , " M2 scaled turn = " , vC2, "\n \n")
RETURN

ScaleTurnThrStr:
vSpeed = vC1 'vC1 = THROTTLE
vTurn = vC2 'vC2 = STEER
IF (vSpeed < 0) THEN
vC2 = (vTurn*vTurnScalingRev)/100
ELSE
vC2 = (vTurn*vTurnScalingFwd)/100
END IF
PRINT ("THROTTLE/STEER scaled turn = " , vC2 , "\n \n")
RETURN

MotorCompensationM1M2:
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'apply compensation to each motor
vC1 = vC1 + (vILeft*vMotorResistance)/1000
vC2 = vC2 + (vIRight*vMotorResistance)/1000
PRINT ("\n" , "vC1 compensated = " , vC1 , " vC2 compensated = " , vC2, "\n \n")
RETURN

MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'do mixing to find motor values (DO NOT TRUNCATE - we will be reversing this below)
vM1 = vC1 + vC2
vM2 = vC1 - vC2
'apply IR compensation
vM1 = vM1 + (vILeft*vMotorResistance)/1000
vM2 = vM2 + (vIRight*vMotorResistance)/1000
'undo mixing to get back new THROTTLE and STEER values
vC1 = (vM1 + vM2)/2
vC2 = (vM1 - vM2)/2
'truncate in case compensation pushed THROTTLE and/or STEER over limits
IF (vC1 > 1000) THEN
vC1 = 1000
END IF
IF (vC1 < -1000) THEN
vC1 = -1000
END IF
IF (vC2 > 1000) THEN
vC2 = 1000
END IF
IF (vC2 < -1000) THEN
vC2 = -1000
END IF
PRINT ("\n" , "compensated THROTTLE = " , vC1 , " compensated STEER = " , vC2, "\n \n")
RETURN

SetMotors:
SetCommand (_G, 1, vC1)
SetCommand (_G, 2, vC2)
PRINT ("\n" , "motor commands set to: THROTTLE = ", vC1, " STEER = ", vC2, "\n \n")
RETURN

[woodygb]

The above contains all 3..you want just 2..no serial?

I think this does just the 2....pulse and analogue.

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

vForwardScaling = 95
vReverseScaling = 35
vTurnScalingFwd = 35
vTurnScalingRev = 35
vMotorResistance = 100

MainLoop:
PRINT ("\n","START OF MAIN LOOP \n")
GoSub FindMode
GoSub SetCommands
PRINT ("\n" , "Joystick THROTTLE = ", vC1, " Joystick STEER = ", vC2, "\n")

'use following three lines if _CIx are M1 and M2 values
' GoSub ScaleSpeedM1M2
' GoSub ScaleTurnM1M2
' GoSub MotorCompensationM1M2

' use following three lines if _CIx are THROTTLE and STEER values
GoSub ScaleSpeedThrStr
GoSub ScaleTurnThrStr
GoSub MotorCompensationThrStr

GoSub SetMotors
Wait (50)
GoTo MainLoop

FindMode:
vP1 = getvalue(_CIP,1)
vP2 = getvalue (_CIP,2)
' vS1 = getvalue(_CIS,1)
' vS2 = getvalue(_CIS,2)
vA1 = getvalue(_CIA,1)
vA2 = getvalue (_CIA,2)
bPactive = TOBOOL (abs(vP1)+abs(vP2) > 0)
bSactive = TOBOOL (abs(vS1)+abs(vS2) > 0)
bAactive = TOBOOL (abs(vA1)+abs(vA2) > 0)
IF bPactive THEN
vMode = 1
ELSEIF bSactive THEN
vMode = 2
ELSEIF bAactive THEN
vMode = 3
ELSE
vMode = 0
END IF
vMode = 1
PRINT ("\n","Mode = ", vMode, "\n \n")
RETURN

SetCommands:
IF vMode = 1 THEN
vC1 = vP1
vC2 = vP2
ELSEIF vMode = 2 THEN
vC1 = vS1
vC2 = vS2
ELSEIF vMode = 3 THEN
vC1 = vA1
vC2 = vA2
ELSE
vC1 = 0
vC2 = 0
END IF
RETURN

ScaleSpeedM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
IF (vSpeed < 0) THEN
vC1 = (vC1*vReverseScaling)/100
vC2 = (vC2*vReverseScaling)/100
ELSE
vC1 = (vC1*vForwardScaling)/100
vC2 = (vC2*vForwardScaling)/100
END IF
PRINT ("\n" , "M1 scaled speed = " , vC1 , " M2 scaled speed = " , vC2, "\n \n")
RETURN

ScaleSpeedThrStr:
vSpeed = vC1 'vC1=THROTTLE
IF (vSpeed < 0) THEN
vScaledSpeed = (vSpeed*vReverseScaling)/100
ELSE
vScaledSpeed = (vSpeed*vForwardScaling)/100
END IF
'set THROTTLE to new speed
vC1= vScaledSpeed
PRINT ("\n" , "THROTTLE/STEER scaled speed = " , vC1 , "\n \n")
RETURN

ScaleTurnM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
'unmix motor settings to find turn value that will be scaled
vTurn = vC1-vC2
IF (vSpeed < 0) THEN
vScaledTurn = (vTurn*vTurnScalingRev)/100
ELSE
vScaledTurn = (vTurn*vTurnScalingFwd)/100
END IF
'apply half of change to each wheel in opposite senses
vChangeTurn = vTurn - vScaledTurn
vC1 = vC1 - vChangeTurn/2 'subtracted
vC2 = vC2 + vChangeTurn/2 'added
PRINT ("M1 scaled turn = " , vC1 , " M2 scaled turn = " , vC2, "\n \n")
RETURN

ScaleTurnThrStr:
vSpeed = vC1 'vC1 = THROTTLE
vTurn = vC2 'vC2 = STEER
IF (vSpeed < 0) THEN
vC2 = (vTurn*vTurnScalingRev)/100
ELSE
vC2 = (vTurn*vTurnScalingFwd)/100
END IF
PRINT ("THROTTLE/STEER scaled turn = " , vC2 , "\n \n")
RETURN

MotorCompensationM1M2:
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'apply compensation to each motor
vC1 = vC1 + (vILeft*vMotorResistance)/1000
vC2 = vC2 + (vIRight*vMotorResistance)/1000
PRINT ("\n" , "vC1 compensated = " , vC1 , " vC2 compensated = " , vC2, "\n \n")
RETURN

MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'do mixing to find motor values (DO NOT TRUNCATE - we will be reversing this below)
vM1 = vC1 + vC2
vM2 = vC1 - vC2
'apply IR compensation
vM1 = vM1 + (vILeft*vMotorResistance)/1000
vM2 = vM2 + (vIRight*vMotorResistance)/1000
'undo mixing to get back new THROTTLE and STEER values
vC1 = (vM1 + vM2)/2
vC2 = (vM1 - vM2)/2
'truncate in case compensation pushed THROTTLE and/or STEER over limits
IF (vC1 > 1000) THEN
vC1 = 1000
END IF
IF (vC1 < -1000) THEN
vC1 = -1000
END IF
IF (vC2 > 1000) THEN
vC2 = 1000
END IF
IF (vC2 < -1000) THEN
vC2 = -1000
END IF
PRINT ("\n" , "compensated THROTTLE = " , vC1 , " compensated STEER = " , vC2, "\n \n")
RETURN

SetMotors:
SetCommand (_G, 1, vC1)
SetCommand (_G, 2, vC2)
PRINT ("\n" , "motor commands set to: THROTTLE = ", vC1, " STEER = ", vC2, "\n \n")
RETURN

[woodygb]

AAArrrgghhh!

There's an errant set mode1 only still in the script...

Here's the 2 modes again ..


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

vForwardScaling = 95
vReverseScaling = 35
vTurnScalingFwd = 35
vTurnScalingRev = 35
vMotorResistance = 100

MainLoop:
PRINT ("\n","START OF MAIN LOOP \n")
GoSub FindMode
GoSub SetCommands
PRINT ("\n" , "Joystick THROTTLE = ", vC1, " Joystick STEER = ", vC2, "\n")

'use following three lines if _CIx are M1 and M2 values
' GoSub ScaleSpeedM1M2
' GoSub ScaleTurnM1M2
' GoSub MotorCompensationM1M2

' use following three lines if _CIx are THROTTLE and STEER values
GoSub ScaleSpeedThrStr
GoSub ScaleTurnThrStr
GoSub MotorCompensationThrStr

GoSub SetMotors
Wait (50)
GoTo MainLoop

FindMode:
vP1 = getvalue(_CIP,1)
vP2 = getvalue (_CIP,2)
' vS1 = getvalue(_CIS,1)
' vS2 = getvalue(_CIS,2)
vA1 = getvalue(_CIA,1)
vA2 = getvalue (_CIA,2)
bPactive = TOBOOL (abs(vP1)+abs(vP2) > 0)
bSactive = TOBOOL (abs(vS1)+abs(vS2) > 0)
bAactive = TOBOOL (abs(vA1)+abs(vA2) > 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

SetCommands:
IF vMode = 1 THEN
vC1 = vP1
vC2 = vP2
ELSEIF vMode = 2 THEN
vC1 = vS1
vC2 = vS2
ELSEIF vMode = 3 THEN
vC1 = vA1
vC2 = vA2
ELSE
vC1 = 0
vC2 = 0
END IF
RETURN

ScaleSpeedM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
IF (vSpeed < 0) THEN
vC1 = (vC1*vReverseScaling)/100
vC2 = (vC2*vReverseScaling)/100
ELSE
vC1 = (vC1*vForwardScaling)/100
vC2 = (vC2*vForwardScaling)/100
END IF
PRINT ("\n" , "M1 scaled speed = " , vC1 , " M2 scaled speed = " , vC2, "\n \n")
RETURN

ScaleSpeedThrStr:
vSpeed = vC1 'vC1=THROTTLE
IF (vSpeed < 0) THEN
vScaledSpeed = (vSpeed*vReverseScaling)/100
ELSE
vScaledSpeed = (vSpeed*vForwardScaling)/100
END IF
'set THROTTLE to new speed
vC1= vScaledSpeed
PRINT ("\n" , "THROTTLE/STEER scaled speed = " , vC1 , "\n \n")
RETURN

ScaleTurnM1M2:
vSpeed = ((vC1 + vC2)*10)/20 'vC1 = left motor, vC2 = right motor
'unmix motor settings to find turn value that will be scaled
vTurn = vC1-vC2
IF (vSpeed < 0) THEN
vScaledTurn = (vTurn*vTurnScalingRev)/100
ELSE
vScaledTurn = (vTurn*vTurnScalingFwd)/100
END IF
'apply half of change to each wheel in opposite senses
vChangeTurn = vTurn - vScaledTurn
vC1 = vC1 - vChangeTurn/2 'subtracted
vC2 = vC2 + vChangeTurn/2 'added
PRINT ("M1 scaled turn = " , vC1 , " M2 scaled turn = " , vC2, "\n \n")
RETURN

ScaleTurnThrStr:
vSpeed = vC1 'vC1 = THROTTLE
vTurn = vC2 'vC2 = STEER
IF (vSpeed < 0) THEN
vC2 = (vTurn*vTurnScalingRev)/100
ELSE
vC2 = (vTurn*vTurnScalingFwd)/100
END IF
PRINT ("THROTTLE/STEER scaled turn = " , vC2 , "\n \n")
RETURN

MotorCompensationM1M2:
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'apply compensation to each motor
vC1 = vC1 + (vILeft*vMotorResistance)/1000
vC2 = vC2 + (vIRight*vMotorResistance)/1000
PRINT ("\n" , "vC1 compensated = " , vC1 , " vC2 compensated = " , vC2, "\n \n")
RETURN

MotorCompensationThrStr:
'find motor currents
vILeft = GetValue (_AIC, 3)
vIRight = GetValue (_AIC, 4)
'do mixing to find motor values (DO NOT TRUNCATE - we will be reversing this below)
vM1 = vC1 + vC2
vM2 = vC1 - vC2
'apply IR compensation
vM1 = vM1 + (vILeft*vMotorResistance)/1000
vM2 = vM2 + (vIRight*vMotorResistance)/1000
'undo mixing to get back new THROTTLE and STEER values
vC1 = (vM1 + vM2)/2
vC2 = (vM1 - vM2)/2
'truncate in case compensation pushed THROTTLE and/or STEER over limits
IF (vC1 > 1000) THEN
vC1 = 1000
END IF
IF (vC1 < -1000) THEN
vC1 = -1000
END IF
IF (vC2 > 1000) THEN
vC2 = 1000
END IF
IF (vC2 < -1000) THEN
vC2 = -1000
END IF
PRINT ("\n" , "compensated THROTTLE = " , vC1 , " compensated STEER = " , vC2, "\n \n")
RETURN

SetMotors:
SetCommand (_G, 1, vC1)
SetCommand (_G, 2, vC2)
PRINT ("\n" , "motor commands set to: THROTTLE = ", vC1, " STEER = ", vC2, "\n \n")
RETURN