Multiple cog access to a ADC routine issue
DiverBob
Posts: 1,110
I discussed an issue I'm having in the robotics forum but I think I have a better chance for success here.
I am running 3 DC motors with feedback via potentiometers for each motor via an MCP3208, 8 channel ADC chip. Additionally, each motor control routine is in a seperate cog so each motor can run independently from the others. There is an ADC routine in the main cog that takes an average of ten readings and returns the ADC value back to the calling routine. The problem is that each motor cog is calling the ADC routine at the same time and this results in bad data being returned
I got a suggestion to use Spin locks but after reading the manual I don't see how they could help in this instance. Does anyone have some suggestions on how to resolve this problem or is there a way to use locks to provide a more orderly access to the ADC routine?
Thanks for the help!
I am running 3 DC motors with feedback via potentiometers for each motor via an MCP3208, 8 channel ADC chip. Additionally, each motor control routine is in a seperate cog so each motor can run independently from the others. There is an ADC routine in the main cog that takes an average of ten readings and returns the ADC value back to the calling routine. The problem is that each motor cog is calling the ADC routine at the same time and this results in bad data being returned
I got a suggestion to use Spin locks but after reading the manual I don't see how they could help in this instance. Does anyone have some suggestions on how to resolve this problem or is there a way to use locks to provide a more orderly access to the ADC routine?
Thanks for the help!
Comments
I would think a similar approach could work in this case. Jhonny+Duane's idea is valid too. One cog is reading the beJesus out of the ADC and storing the results where any other cog can come and get it. You can then use filtering on the results too. This works fine as long as that SPI bus is dedicated to the ADC.
That was the idea one of the guys in the office had. I could run the ADC routine in a seperate cog, read all 8 inputs and store them in global variables. I already have the motor cogs monitoring variables for determining when to take action. The only disadvantage of this is that I'll have 4 cogs dedicated to the motors and reading ADC values, a cog for floating Point calcs, a cog for serial comms, and one cog for the main loop. This only leaves 1 cog for any other functionality. First time I've come this close to running out of processors!
I'll give this a shot tonight and see how well it works. Should I do anything to 'lock' a global variable just prior to reading it in case the ADC routine is trying to update it while the motor routine is reading the value?
Bob
So long as the whole value you need is stored in a single LONG there is no need. Hub round robin means that only one cog can be reading or writing that LONG at each access window.
I output a PWM to a HB25 to control speed and direction of the motor. A potentiometer provides the feedback mechanism. The code for this receives an angle input. The software then translates the angle to a value. The actual ADC value is compared to the desired position to determine direction. Next it starts the motor at full speed in that direction while watching the changing ADC values. Once it gets close to the desired value, the speed is gradually reduced and the stopped at the desired ADC value. That's the basic operation, a series of repeats while testing for specific conditions.
I'll post my current motor control code when I get back to my other computer so you can see how it works. I didn't get an opportunity to work on the code last night but we are in a winter storm warning for the next two days so I should have lots of time to experiment!
Bob
As I read your post, though, I'm thinking "HB25s are set-and-forget" -- why use a cog at all? I coded a pan/zoom controller for a friend that uses an MCP3204 and two servos (to drive the lens rings). The code is setup to run in a 20ms loop. In the top of the loop I read the ADC four times and do an average, then I drop through and create pulses for the servos using one of the counters. Easy-peasy.
The real question is: Are you connecting a pot to a motor that could be programmed to run past its limit? This could result in damage. What I would suggest is attaching an encoder to the motor shaft. On another project for the same friend, we control three DC motors on a pan/tilt/trolley camera rig. We need two cogs for motors (because we're providing signals to the H-Bridge) and one cog for the encoders. There is another cog that is used for speed and location management. We know from empirical testing the how to convert encoder counts in a given period to speed. We can also use the encoder value for location. In the end that lets the user create a camera movement that we can record and playback.
This is the product.
-- http://cameraturret.com/genesis.htm
This company started doing camera controls with the BASIC Stamp, migrated to the SX (with my help), and is now using the Propeller (also with my help).
I would be interested in seeing the code loop you used for controlling multiple DC motors if possible. Thanks for the good ideas. I've already had some success with putting the ADC routine in its own cog and having it continuously updating the position variables.
'' ================================================================================================= '' '' File....... Hexapod IK Test 1.spin '' Purpose.... Create series of test routines for leg control using IK '' Author..... Bob Sweeney '' Started.... 12/29/13 '' '' ================================================================================================= con _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 CLK_FREQ = ((_clkmode - xtal1) >> 6) * _xinfreq MS_001 = CLK_FREQ / 1_000 con #1, HOME, #8, BKSP, TAB, LF, CLREOL, CLRDN, CR, #16, CLS ' PST formmatting control con RX1 = 31 TX1 = 30 SDA = 29 SCL = 28 CS = 7 CLK = 8 DIO = 9 femur = 16 tibia = 17 coxa = 18 Degrees = (180.0/pi) Radians = (pi / 180.0) TravelDeadZone = 0.5 Trigger5 = 0 CoxaLength = 2.5 FemurLength = 6.0 TibiaLength = 20.625 CoxaAngle = 90.0 obj adc : "jm_mcp3208_ez" motor : "Servo32v9.spin" F : "Float32Full" pst : "Parallax Serial Terminal" var word Femurcounter, TibiaCounter, CoxaCounter long CoxaAngle1, FemurAngle1, TibiaAngle1 long VPosX, VPosY, VPosZ long femurStack[200], tibiaStack[200], coxaStack[200], adcstack[100] word FemurADC, TibiaADC, CoxaADC word FemurDone, TibiaDone, CoxaDone pub Start | angle1, angle2, angle3 F.Start pst.Start(115200) ' Start the Parallax Serial Terminal cog motor.start cognew(GetADC, @adcstack) dira[femur] := 0 'femur motor output dira[tibia] := 0 'tibia motor output dira[coxa] := 0 'coxa motor output repeat until ina[femur] == 1 repeat until ina[tibia] == 1 repeat until ina[coxa] == 1 waitcnt(MS_001 + cnt) ' let objects load FemurDone := 0 TibiaDone := 0 CoxaDone := 0 IKFemurAngle := 90 IKTibiaAngle := 90 IKCoxaAngle := 0 ' cognew(FemurMotorRun, @femurStack) cognew(TibiaMotorRun, @tibiaStack) ' cognew(CoxaMotorRun, @coxaStack) waitcnt(100000 + cnt) 'let objects load repeat repeat while FemurDone == 1 AND TibiaDone == 1 AND CoxaDone == 1 'wait for motor move to complete EnterAngle pub EnterAngle | a1, a2, a3 a1 := 0 a2 := 0 a3 := 0 ' pst.Str(String(pst#NL, "Enter Coxa angle: ")) ' Prompt user to enter value ' pst.Dec(a1) ' a1 := pst.DecIn pst.Str(String(pst#NL, "Enter Tibia angle: ")) ' Prompt user to enter value pst.Dec(a2) a2 := pst.DecIn ' Get value ' pst.Str(String(pst#NL, "Enter Femur angle: ")) ' Prompt user to enter value ' pst.Dec(a3) ' a3 := pst.DecIn ' Get value ' IKCoxaAngle := a1 IKTibiaAngle := a2 ' IKFemurAngle := a3 repeat while (coxadone == 0) OR (femurdone == 0) OR (tibiadone == 0) pub FemurMotorRun | TargetADC, ActualADC, initialADC, t1, t2, error, speed ' input ADC value via PST and the motor moves to that angle dira[femur] := 1 'femur motor output repeat Femurcounter := 1 error := 0 waitcnt(200000 + cnt) 'let objects load FemurDone := 0 'convert angle to ADC value and return result if IKFemurAngle < femurMinAngle 'setup default min and max angles IKFemurAngle := femurMinAngle elseif IKFemurAngle > femurMaxAngle IKFemurAngle := femurMaxAngle t1 := (femurMaxADC - femurMinADC) * (IKFemurAngle - femurMinAngle) t2 := t1 / (femurMaxAngle - femurMinAngle) TargetADC := ||t2 '+ femurMinADC ActualADC := femurADC initialADC := ActualADC { pst.Str(String(pst#NL, "Input Angle: ")) pst.Dec(inputAngle) pst.Str(String(pst#NL, "Start Femur ADC: ")) pst.Dec(ActualADC) pst.Str(String(pst#NL, "Target ADC: ")) pst.Dec(TargetADC)} t1 := ||(TargetADC - ActualADC) if t1 > 30 'get absolute value of change if TargetADC < ActualADC 'determine movement direction, move up speed := 2000 repeat until ||(TargetADC - ActualADC) =< 50 'repeat full speed until ramp down motor.Set(femur, speed) ' pst.Str(String(pst#NL, "Max Femur up speed: ")) ' pst.Dec(ActualADC) ' pst.Str(String(" - speed: ")) ' pst.Dec(speed) ActualADC := femurADC if FemurStopError(initialADC, ActualADC) 'stop if ADC not changing motor.Set(femur, 1500) error := 1 quit repeat until TargetADC => (ActualADC - 25) 'start down ramp if error == 1 quit motor.Set(femur, speed) ActualADC := FemurADC ' pst.Str(String(pst#NL, "Move Femur up - Ramping down: ")) ' pst.Dec(ActualADC) ' pst.Str(String(" - speed: ")) ' pst.Dec(speed) speed := speed - 50 'decrement count t2 := speed t2 <#= 2000 'max speed 2000 speed := t2 #>= 1550 'min speed 1600 if FemurStopError(initialADC, ActualADC) 'stop if ADC not changing motor.Set(femur, 1500) quit if (ActualADC == 0) 'stop motor motor.Set(femur, 1500) quit motor.Set(femur, 1500) else 'move down speed := 1000 'set start speed repeat until ||(TargetADC - ActualADC) =< 50 'repeat full speed until ramp down motor.Set(femur, speed) ' pst.Str(String(pst#NL, "Max Femur down speed: ")) ' pst.Dec(ActualADC) ' pst.Str(String(" - speed: ")) ' pst.Dec(speed) ActualADC := femurADC if FemurStopError(initialADC, ActualADC) 'stop if ADC not changing motor.Set(femur, 1500) error :=1 quit repeat until TargetADC =< (ActualADC) 'start down ramp if error == 1 quit motor.Set(femur, speed) ActualADC := femurADC ' pst.Str(String(pst#NL, "Move Femur down - Ramping down: ")) ' pst.Dec(ActualADC) ' pst.Str(String(" - speed: ")) ' pst.Dec(speed) speed := speed + 50 'decrement count t2 := speed t2 #>= 1000 'min speed 1000 speed := t2 <#= 1450 'max speed 1400 if FemurStopError(initialADC, ActualADC) 'stop if ADC not changing motor.Set(femur, 1500) quit if (ActualADC == 0) 'error catch - stop motor motor.Set(femur, 1500) quit motor.Set(femur, 1500) motor.Set(femur, 1500) FemurDone := 1 'ready to continue { pst.Str(String(pst#NL, "STOP Femur ")) pst.Str(String(pst#NL, "Entered angle: ")) pst.Dec(inputangle) pst.Str(String(pst#NL, "Target ADC: ")) pst.Dec(TargetADC) pst.Str(String( " - Final ADC: ")) pst.Dec(ActualADC) pst.Chars(pst#NL, 2) } pub FemurStopError(iADC, aADC) FemurCounter := FemurCounter + 1 'count to 5 if FemurCounter == 1 'at start get ADC value iADC := aADC if FemurCounter == 5 'max count 5 FemurCounter := 0 if iADC == aADC 'error, ADC not changing Result := true pub TibiaStopError(iADC, aADC) TibiaCounter := TibiaCounter + 1 'count to 5 if TibiaCounter == 1 'at start get ADC value iADC := aADC if TibiaCounter == 5 'max count 5 TibiaCounter := 0 if iADC == aADC 'error, ADC not changing Result := true pub CoxaStopError(iADC, aADC) CoxaCounter := CoxaCounter + 1 'count to 5 if CoxaCounter == 1 'at start get ADC value iADC := aADC if CoxaCounter == 5 'max count 5 CoxaCounter := 0 if iADC == aADC 'error, ADC not changing Result := true pub TibiaMotorRun | TargetADC, ActualADC, initialADC, t1, t2, error, speed ' input ADC value via PST and the motor moves to that angle dira[tibia] := 1 'femur motor output repeat TibiaCounter := 1 error := 0 waitcnt(200000 + cnt) 'let objects load TibiaDone := 0 'convert angle to ADC value and return result if IKTibiaAngle < tibiaMinAngle 'setup default min and max angles IKTibiaAngle := tibiaMinAngle elseif IKTibiaAngle > tibiaMaxAngle IKTibiaAngle := tibiaMaxAngle t1 := (tibiaMaxADC - tibiaMinADC) * (IKTibiaAngle - tibiaMinAngle) t2 := t1 / ||(tibiaMaxAngle - tibiaMinAngle) TargetADC := ||t2 '+ tibiaMinADC ActualADC := TibiaADC initialADC := ActualADC { pst.Str(String(pst#NL, "Input Angle: ")) pst.Dec(IKTibiaAngle) pst.Str(String(pst#NL, "Start Tibia ADC: ")) pst.Dec(ActualADC) pst.Str(String(pst#NL, "Target ADC: ")) pst.Dec(TargetADC)} t1 := ||(TargetADC - ActualADC) if t1 > 40 'get absolute value of change if TargetADC < ActualADC 'determine movement direction, move up speed := 2000 'set start speed repeat until ||(TargetADC - ActualADC) =< 75 'repeat full speed until ramp down Motor.Set(tibia, speed) pst.Str(String(pst#NL, "Max Tibia speed in: ")) pst.Dec(ActualADC) pst.Str(String(" - speed: ")) pst.Dec(speed) ActualADC := TibiaADC if TibiaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(tibia, 1500) error := 1 quit repeat until TargetADC => (ActualADC - 75) 'start down ramp if error := 1 quit Motor.Set(tibia, speed) ActualADC := TibiaADC pst.Str(String(pst#NL, "Move Tibia in - Ramping down: ")) pst.Dec(ActualADC) pst.Str(String(" - speed: ")) pst.Dec(speed) speed := speed - 50 'decrement count t2 := speed t2 <#= 2000 'max speed 2000 speed := t2 #>= 1550 'min speed 1600 if TibiaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(tibia, 1500) quit if (ActualADC == 0) 'stop motor Motor.Set(tibia, 1500) quit ActualADC := TibiaADC Motor.Set(tibia, 1500) else 'move down speed := 1000 'set start speed repeat until ||(TargetADC - ActualADC) =< 75 'repeat full speed until ramp down Motor.Set(tibia, speed) pst.Str(String(pst#NL, "Max Tibia speed out: ")) pst.Dec(ActualADC) pst.Str(String(" - speed: ")) pst.Dec(speed) ActualADC := TibiaADC if TibiaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(tibia, 1500) error := 1 pst.Str(String(pst#NL, "Tibia Error: ")) pst.Dec(initialADC) quit repeat until TargetADC =< (ActualADC + 50) 'start down ramp if error == 1 quit Motor.Set(tibia, speed) ActualADC := TibiaADC pst.Str(String(pst#NL, "Move Tibia out - Ramping down: ")) pst.Dec(ActualADC) pst.Str(String(" - speed: ")) pst.Dec(speed) speed := speed + 50 'decrement count t2 := speed t2 #>= 1000 'min speed 1000 speed := t2 <#= 1450 'max speed 1400 if TibiaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(tibia, 1500) quit if (ActualADC == 0) 'error catch - stop motor Motor.Set(tibia, 1500) quit ActualADC := TibiaADC Motor.Set(tibia, 1500) Motor.Set(tibia, 1500) TibiaDone := 1 { actualADC := TibiaADC pst.Str(String(pst#NL, "STOP Tibia")) pst.Str(String(pst#NL, "Entered angle: ")) pst.Dec(IKTibiaAngle) pst.Str(String(pst#NL, "Target ADC: ")) pst.Dec(TargetADC) pst.Str(String( " - Final ADC: ")) pst.Dec(ActualADC) actualADC := tibiaADC pst.Str(String(pst#NL, "Final ADC: ")) pst.Dec(ActualADC) pst.Chars(pst#NL, 2) } pub CoxaMotorRun | TargetADC, ActualADC, initialADC, t1, t2, error, speed ' input ADC value via PST and the motor moves to that angle dira[coxa] := 1 'coxa motor output repeat CoxaCounter := 1 'used for error catching error := 0 waitcnt(200000 + cnt) 'let objects load CoxaDone := 0 'flag when move is complete 'convert angle to ADC value and return result IKCoxaAngle := IKCoxaAngle + 90 'convert from range +60 - -60 degrees if IKCoxaAngle < coxaMinAngle 'setup default min and max angles IKCoxaAngle := coxaMinAngle elseif IKCoxaAngle > coxaMaxAngle IKCoxaAngle := coxaMaxAngle t1 := (coxaMaxADC - coxaMinADC) * (IKCoxaAngle - coxaMinAngle) 'calculate ADC target based on t2 := t1 / ||(coxaMaxAngle - coxaMinAngle) 'slope of line; dY/dX TargetADC := ||t2 + coxaMinADC 'add in minimum adc value ActualADC := CoxaADC 'get current ADC value initialADC := ActualADC 'save starting valve t1 := ||(TargetADC - ActualADC) 'setup deadzone value if t1 > 30 'get absolute value of change if TargetADC > ActualADC 'determine movement direction, move up speed := 1800 'set start max speed repeat until ||(TargetADC - ActualADC) =< 75 'repeat full speed until ramp down Motor.Set(coxa, speed) ActualADC := CoxaADC 'get adc value if CoxaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(coxa, 1500) error := 1 'error occurred quit 'exit loop repeat until TargetADC => (ActualADC - 75) 'start down ramp if error == 1 quit Motor.Set(coxa, speed) ActualADC := CoxaADC speed := speed - 50 'decrement count t2 := speed t2 <#= 2000 'max speed 2000 speed := t2 #>= 1550 'min speed 1550 if CoxaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(coxa, 1500) quit if (ActualADC == 0) 'stop motor Motor.Set(coxa, 1500) quit ActualADC := CoxaADC Motor.Set(coxa, 1500) else 'move down speed := 1200 'set start max speed repeat until ||(TargetADC - ActualADC) =< 50 'repeat full speed until ramp down Motor.Set(coxa, speed) ActualADC := CoxaADC if CoxaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(coxa, 1500) error := 1 quit repeat until TargetADC =< (ActualADC + 25) 'start down ramp if error == 1 quit Motor.Set(coxa, speed) ActualADC := CoxaADC speed := speed + 50 'decrement count t2 := speed t2 #>= 1000 'min speed 1000 speed := t2 <#= 1450 'max speed 1400 if CoxaStopError(initialADC, ActualADC) 'stop if ADC not changing Motor.Set(coxa, 1500) quit if (ActualADC == 0) 'error catch - stop motor Motor.Set(coxa, 1500) quit ActualADC := CoxaADC Motor.Set(coxa, 1500) Motor.Set(coxa, 1500) CoxaDone := 1 pub GetADC | i, n, ch adc.start(CS, CLK, DIO) ' start adc driver repeat repeat ch from 0 to 2 i := 0 repeat n from 0 to 9 'average readings over 10 cycles i := i + adc.read(ch, 1) case ch 0: femurADC := i/10 1: tibiaADC := i/10 2: coxaADC := i/10 dat femurMinAngle word 35 femurMaxAngle word 142 femurMinADC word 0 femurMaxADC word 3650 tibiaMinAngle word 86 tibiaMaxAngle word 143 tibiaMinADC word 3840 tibiaMaxADC word 70 coxaMinAngle word 30 coxaMaxAngle word 150 coxaMinADC word 1440 coxaMaxADC word 2235It would have been useful to see your code. As that's not available yet, I knocked up this little demo to give you some ideas. This program uses just the main cog. The ADC is read and averaged at the top of the loop, those values are converted to speed/direction values (in microseconds), then the motors are updated. The loop runs every 50ms (20x/second).
Tip: For averaging it's useful to select a loop count that's a power of 2 (I used 4); this lets you divide by using a right shift which is much faster than division.
[Edit] You posted your code while I was posting -- I'll have a look and update my suggestion if warranted.
Question: With the pots tied to the motors you know what the range limits are, correct?
While working on the pan/tilt/trolley project I called on one of my friends here in Hollywood as he had built a lot of motorized camera rigs for Disney. He explained an easy way to accelerate from a starting point and then decelerate into a target point. This works when you have absolute position values (as we did using the encoders, and you should with the pots).
In simple terms the move is divided into four phases:
-- A: Accelerate up to desired max speed, then calculate delta to mid-point
-- B: Run to mid-point of move
-- C: Run to deceleration point (value calculated from end of phase A)
--
In phase A of the move the speed is ramped up to the desired maximum. When this is reached, the distance between the current position and the mid-point of the move is calculated. The motor runs to the mid-point (phase
In practice I divided phase D into 3 sections: 80% of the phase D distance ran at the normal deceleration rate; then I slowed the rate for another 15%; the final 5% of the move ran very slowly. By doing this we never had overshoot and we were able to get the pan/tilt/trolley rig to hit marks dead on in multiple passes. The code was fairly simple using a state-machine for each motor, with phase D being divided into three sub-states.
I've attached my servo object for your consideration: it has built in ramping (microseconds change per second of real time) and a method that will return true when a ramping channel is at the target position; this feature would be useful in implementing the trapezoidal move described above.
Forgive me if this is overly simplistic for what you're doing; I'm only offering this up in the hopes of being helpful.
I want to give a go at coding using a single loop like your previous post example and see how well that responds. The acceleration/ deceleration process you described sounds good also, I'm not happy with what I have right now. I even removed the acceleration code to cut down on the complexity.
Thank you, these are all good ideas that allow me to explore some different ways to implement the code.
The CoxaMotorRun cog monitors the global value CoxaAngle. It determines the target ADC value and moves the motor until it gets within a deadzone around the value. The calculation for the targetADC is based on the assumption that the slope of the potentiometer output is linear. The routine GetTarget had some issues and wouldn't work right until I broke out the Y1, Y2, X1 and X2 values and converted them to float outside of the equation. Once I did that everything started working right. Haven't gone back to figure out what was happening there.
Where the speed is manually set to 1300 and 1700 is where I need to add the ramping functions. I spent a few hours searching the forum for examples. I liked the sine wave and S curve approaches to ramping, just haven't figured out how to implement yet. I'll work on that later, I want to use this same layout for the Tibia motor and get both motors running together when sent angle commands. Its getting closer to what I am looking for.
'' ================================================================================================= '' '' File....... Hexapod Leg Test 2.spin '' Purpose.... Create routines for leg control '' Author..... Bob Sweeney '' Started.... 12/29/13 '' '' ================================================================================================= con _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 CLK_FREQ = ((_clkmode - xtal1) >> 6) * _xinfreq MS_001 = CLK_FREQ / 1_000 con #1, HOME, #8, BKSP, TAB, LF, CLREOL, CLRDN, CR, #16, CLS ' PST formmatting control con RX1 = 31 TX1 = 30 SDA = 29 SCL = 28 CS = 7 CLK = 8 DIO = 9 femur = 16 tibia = 17 coxa = 18 Degrees = (180.0/pi) Radians = (pi / 180.0) stopmotor = 1500 obj adc : "jm_mcp3208_ez" ' motor : "Servo32v9.spin" F : "Float32Full" pst : "Parallax Serial Terminal" var long CoxaAngle long coxaStack[200], adcstack[100] long pot[8] word CoxaADC, CoxaDone, CoxaCounter pub Start | n F.Start pst.Start(115200) ' Start the Parallax Serial Terminal cog motor.start cognew(GetADC, @adcstack) dira[coxa] := 0 'coxa motor output repeat until ina[coxa] == 1 waitcnt(10000000 + cnt) ' let objects load CoxaDone := 0 CoxaAngle := 0 cognew(CoxaMotorRun, @coxaStack) waitcnt(100000 + cnt) 'let objects load n := 0 repeat ' EnterAngle case n 0: coxaAngle := 0 1: coxaAngle := -60 2: coxaAngle := 60 3: coxaAngle := 60 4: coxaAngle := -60 5: coxaAngle := 60 6: coxaAngle := -60 7: coxaAngle := 60 8: coxaAngle := -60 n++ if n == 9 n := 0 waitcnt(320000000 + cnt) 'let objects load pub EnterAngle pst.Str(String(pst#NL, "Enter Coxa angle: ")) ' Prompt user to enter value CoxaAngle := pst.DecIn pst.Dec(CoxaAngle) pub CoxaMotorRun | TargetADC, ActualADC, speed, workangle ' input ADC value via PST and the motor moves to that angle dira[coxa] := 1 'coxa motor output repeat ' coxaangle := 60 CoxaAngle := -60 #> CoxaAngle <# 60 'constrain input workangle := CoxaAngle + 90 'convert from range +60 - -60 degrees 'calculate ADC value for input angle TargetADC := GetTarget(workangle) ActualADC := CoxaADC 'get current ADC value if ||(ActualADC - TargetADC) > 10 ActualADC := CoxaADC 'get current ADC value if (ActualADC - TargetADC) > 0 'determine motor direction speed := 1300 else speed := 1700 motor.Set(coxa, speed) else motor.Set(coxa, stopmotor) 'stop motor, inside deadzone pub GetTarget(angle)| y1, y2, x1, x2, m, b 'calculate ADC value using slope of line, y=mx+b where m = y1-y0/x1-x0 y1 := f.ffloat(coxaMinADC) y2 := f.ffloat(coxaMaxADC) x1 := f.ffloat(coxaMinAngle) x2 := f.ffloat(coxaMaxAngle) m := f.fdiv(f.fsub(y2,y1), f.fsub(x2, x1)) b := f.fsub(y2,f.fmul(x2, m)) result := f.fTrunc(f.fadd(f.fmul(m, f.ffloat(angle)), b)) pub GetADC | i, n, ch adc.start(CS, CLK, DIO) ' start adc driver repeat coxaADC := adc.read(2, 1) dat coxaMinAngle word 30 coxaMaxAngle word 150 coxaMinADC word 1440 coxaMaxADC word 2240 {{ Terms of Use: MIT License Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. }}