Controlling a large motor with a 28X2

Gramps

Senior Member
;This is Test circuit for the DC Motor Driver MD20A copied from page 173 in Manual 2 covering the pwmduty command

;C.4 connects to direction pin on MD20A
;C.2 connects to PWM pin on MD20A
;pmduty is 0 to 1023
Code:
#picaxe 08m2
#no_data

init:    pwmout C.2,100,150    ; start pwm

main:  high C.4        ;CCW Rotation
    pwmduty C.2,300        ; set pwm duty
    pause 3000        ; pause 3 s
    low C.4        ;CW Rotation
    pause 3000        ; pause 3 s
    goto main    ; loop back to start
Here is the link to the video test.

We're pretty happy with the test. It actually works!
Here we want to change up to a 28X2...........
Pictured below is the 5k slave pot mounted at the pivot point of the large gear. The three leads can also be seen;

23564

With 5 volts across the slave pot, the wiper reads 1.2 volts (1.12k) full left side, 2.5 volts (2.41k) center, and 3.7 volts (3.65k) to the far right side.
The plan is to move the slave pot (B.0) with a 5k control pot (B.1) using a pwmduty command to the MD20A motor driver (C.1)
The MD20A also has a direction pin (B.7) which is high for CCW and low for CW rotation.
 
Last edited:

hippy

Senior Member
If I have understood what you have and want correctly; I presume you want to be able to move your mechanical contraption such that it points how your target pot is set. So you set the desired position on the control pot and turn the motor until the feedback pot matches.

There are going to be a number of parts to that. You won't need to use a comparator input because the pot control and matching will be done in software.

The first thing which needs to be done is to read both pots and normalise those, so both have the same values when fully left, centred and fully right. The desired position pot should be simple enough as it will have full range 0 to 1023 when read with READADC10. The feedback pot reading will need processing to normalise it because it has a restricted range.

Once normalised the two pot values can be compared and which direction the motor has to be turned in can be determined. The motor can then be turned with speed control and stopping overlaid on top.

There are going to be a number of program parts to all that but it can be approached from a top-down perspective where the parts can all be added and tested incrementally. The main program will probably be something like -
Code:
Do
  Gosub ReadDesiredPostion
  Gosub ReadCurrentPosition
  Select Case currentPosition
    Case  < desiredPosition : Gosub TurnClockwise
    Case  > desiredPosition : Gosub TurnAntiClockwise
    Else                    : Gosub StopTurning
  End Select
Loop
Then it's just a matter of creating the routines to do those things and adding the variables used to do it.

The first step would be to create the ReadDesiredPostion and ReadCurrentPosition routines, check they work. I will have a think about the maths required to normalise the feedback pot value.
 

Gramps

Senior Member
Hippy, Thanks for your help! Yes, you are correct. That is exactly what we are trying to accomplish.
Gramps
 

Gramps

Senior Member
Forgive me for bumping this thread but please give me a hint or two as at how to proceed. Thanks, Gramps
 

hippy

Senior Member
Normalising the pots, making the desired position and feedback position cover the same range proved harder to do than I imagined, or my brain wasn't in the right place to do that.

Once can either scale down the desired pot to match the feedback pot range, or scale up the feedback pot to match the desired pot range. Both options pose challenges for avoiding overflow of 16-bits.

This does the first, scales down the desired pot to match the feedback pot.
Code:
#Picaxe 28X2

Symbol DESIRED_POT     = C.0
Symbol FEEDBACK_POT    = C.1

; N = Vin * 1023 / Vpsu

Symbol FEEDBACK_TOP     = 757 ; 3.7V
Symbol FEEDBACK_MID     = 512 ; 2.5V
Symbol FEEDBACK_BOT     = 245 ; 1.2V

Symbol FEEDBACK_TOP_GAP = FEEDBACK_TOP - FEEDBACK_MID
Symbol FEEDBACK_BOT_GAP = FEEDBACK_MID - FEEDBACK_BOT

Symbol reserveW0        = w0 ; b1:b0
Symbol desiredPosition  = w1 ; b3:b2
Symbol currentPosition  = w2 ; b5:b4

#IfDef SIMULATING
  SerTxd("Testing", CR, LF )
  SerTxd("FEEDBACK_TOP     = ", #FEEDBACK_TOP, CR, LF )
  SerTxd("FEEDBACK_MID     = ", #FEEDBACK_MID, CR, LF )
  SerTxd("FEEDBACK_BOT     = ", #FEEDBACK_BOT, CR, LF )
  SerTxd("FEEDBACK_TOP_GAP = ", #FEEDBACK_TOP_GAP, CR, LF )
  SerTxd("FEEDBACK_BOT_GAP = ", #FEEDBACK_BOT_GAP, CR, LF )
  w0 = 1023 : Gosub Test
  w0 =  768 : Gosub Test
  w0 =  512 : Gosub Test
  w0 =  256 : Gosub Test
  w0 =    0 : Gosub Test
  End
#EndIf
Do
  Gosub ReadDesiredPostion
  Gosub ReadCurrentPosition
  Select Case currentPosition
    Case  > desiredPosition : Gosub TurnAntiClockwise
    Case  < desiredPosition : Gosub TurnClockwise
    Else                    : Gosub StopTurning
  End Select
Loop

ReadDesiredPostion:
  ReadAdc10 DESIRED_POT, w0
CalcDesiredPosition:
  If w0 >= 512 Then
    w0 = w0 - 512 / 4
    w0 = w0 * FEEDBACK_TOP_GAP / 128
    w0 = w0 + FEEDBACK_MID
  Else
    w0 = 512 - w0 / 4
    w0 = w0 * FEEDBACK_BOT_GAP / 128
    w0 = FEEDBACK_MID - w0
  End If
  desiredPosition = w0
  Return

ReadCurrentPosition:
  ReadAdc10 FEEDBACK_POT, w0
CalcCurrentPosition:
  currentPosition = w0
  Return

TurnAntiClockwise:
  Return

TurnClockwise:
  Return

StopTurning:
  Return

#IfDef SIMULATING
Test:
  SerTxd("Desired Nadc = ", #w0, TAB )
  Gosub CalcDesiredPosition
  SerTxd( #desiredPosition, CR, LF )
  Return
#EndIf
 

hippy

Senior Member
This seems to be the alternative to scale the feedback pot to give a 0-512-1023 normalisation -
Code:
#Picaxe 28X2

Symbol DESIRED_POT     = C.0
Symbol FEEDBACK_POT    = C.1

; N = Vin * 1023 / Vpsu

Symbol FEEDBACK_TOP     = 757 ; 3.7V
Symbol FEEDBACK_MID     = 512 ; 2.5V
Symbol FEEDBACK_BOT     = 245 ; 1.2V

Symbol FEEDBACK_TOP_GAP = FEEDBACK_TOP - FEEDBACK_MID
Symbol FEEDBACK_BOT_GAP = FEEDBACK_MID - FEEDBACK_BOT

Symbol reserveW0        = w0 ; b1:b0
Symbol desiredPosition  = w1 ; b3:b2
Symbol currentPosition  = w2 ; b5:b4

#IfDef SIMULATING
  SerTxd("Testing", CR, LF )
  SerTxd("FEEDBACK_TOP     = ", #FEEDBACK_TOP, CR, LF )
  SerTxd("FEEDBACK_MID     = ", #FEEDBACK_MID, CR, LF )
  SerTxd("FEEDBACK_BOT     = ", #FEEDBACK_BOT, CR, LF )
  SerTxd("FEEDBACK_TOP_GAP = ", #FEEDBACK_TOP_GAP, CR, LF )
  SerTxd("FEEDBACK_BOT_GAP = ", #FEEDBACK_BOT_GAP, CR, LF )
  w0 = FEEDBACK_TOP : Gosub Test
  w0 = FEEDBACK_TOP - FEEDBACK_MID / 2 + FEEDBACK_MID : Gosub Test
  w0 = FEEDBACK_MID : Gosub Test
  w0 = FEEDBACK_MID - FEEDBACK_BOT / 2 + FEEDBACK_BOT : Gosub Test
  w0 = FEEDBACK_BOT : Gosub Test
  End
#EndIf
Do
  Gosub ReadDesiredPostion
  Gosub ReadCurrentPosition
  Select Case currentPosition
    Case  > desiredPosition : Gosub TurnAntiClockwise
    Case  < desiredPosition : Gosub TurnClockwise
    Else                    : Gosub StopTurning
  End Select
Loop

ReadDesiredPostion:
  ReadAdc10 DESIRED_POT, w0
CalcDesiredPosition:
  desiredPosition = w0
  Return

ReadCurrentPosition:
  ReadAdc10 FEEDBACK_POT, w0
CalcCurrentPosition:
  If w0 >= FEEDBACK_MID Then
    w0 = w0 - FEEDBACK_MID
    w0 = w0 * 128 / FEEDBACK_TOP_GAP * 4 Max 511
    w0 = w0 + 512
  Else
    w0 = FEEDBACK_MID - w0
    w0 = w0 * 128 / FEEDBACK_BOT_GAP * 4 Max 512
    w0 = 512 - w0
  End If
  currentPosition = w0
  Return

TurnAntiClockwise:
  Return

TurnClockwise:
  Return

StopTurning:
  Return

#IfDef SIMULATING
Test:
  SerTxd("Current Nadc = ", #w0, TAB )
  Gosub CalcCurrentPosition
  SerTxd( #currentPosition, CR, LF )
  Return
#EndIf
 

Gramps

Senior Member
Hippy, what an awesome piece of code! Had no idea it would be this complicated.
The electronics in a standard servo are doing a lot more work than I realized.
Here is my beginner's mind thinking,
We can see with the very small range we are working with of 0 to 5 volts, that the regulation would have to be very tight so we don't get too much drift. But
Why doesn't
"if control pot A reads 2.5 volts then set slave pot B to 2.5 volts" work?
Then scale the pots so the voltages match not being too concerned about what the numbers are?
Another idea comes to mind. Use a 2X08 to read the slave pot on each motor (there are 3 motors) and use a 2X28 to tell the appropriate motor driver what to do?
 
Last edited:

hippy

Senior Member
Why doesn't "if control pot A reads 2.5 volts then set slave pot B to 2.5 volts" work?
It probably would but then you are losing almost half the resolution of the control pot, the bottom and top quarter of the pot would have no effect.
Code:
ReadDesiredPostion:
  ReadAdc10 DESIRED_POT, w0
CalcDesiredPosition:
  desiredPosition = w0 Min FEEDBACK_BOT Max FEEDBACK_TOP
  Return

ReadCurrentPosition:
  ReadAdc10 FEEDBACK_POT, w0
CalcCurrentPosition:
  currentPosition = w0
  Return
 

hippy

Senior Member
This, untested, should provide basic motor control. You might have to swap the <= and >= depending on which way the motor turns when the feedback pot increases.
Code:
Symbol  DIRECTION     = C.4
#Define CLOCKWISE     Low  DIRECTION
#Define ANTICLOCKWISE High DIRECTION

Symbol  MOTOR         = C.2
Symbol  MOTOR_PERIOD  = 100
Symbol  MAX_SPEED     = 300

Symbol  speed         = w3 ; b7:b6

TurnAntiClockwise:
  If currentPosition <= desiredPosition Then
    Gosub StopTurning
  Else If speed = 0 Then
    speed = MAX_SPEED
    ANTICLOCKWISE
    PwmOut MOTOR, MOTOR_PERIOD, speed
  Else
    PwmDuty MOTOR, speed
  End If
  Return

TurnClockwise:
  If currentPosition >= desiredPosition Then
    Gosub StopTurning
  Else If speed = 0 Then
    speed = MAX_SPEED
    CLOCKWISE
    PwmOut MOTOR, MOTOR_PERIOD, speed
  Else
    PwmDuty MOTOR, speed
  End If
  Return

StopTurning:
  PwmOut MOTOR, OFF
  speed = 0
  Return
 

hippy

Senior Member
We are getting an error;
How is "DESIRED_POT" defined in its SYMBOL statement ?

Noting it's a 28X2 I probably got it wrong with defining it as "C.0" when it should be an ADC channel number. What pins are the two pots connected to ?
 

Gramps

Senior Member
#Picaxe 28X2 Symbol DESIRED_POT = C.0 Symbol FEEDBACK_POT = C.1 ; N = Vin * 1023 / Vpsu
[/ quote
It looks like I got it wrong because I did not add the first section of code you wrote to the last section. Let me re copy and paste this and I'll get back to you later today
 

lbenson

Senior Member
In making your corrections, note that on the 28x2, C.0 and C.1 are not ADC pins. And remember that when using a variable or constant with ADC on the X2s, you must specify the channel number, not the pin number, e.g., 14 for C.2, 4 for C.3, etc., as shown with the pinouts in manual 1.
 

Gramps

Senior Member
The code compiles correctly when we combined the two sections of code and commented out these duplicate names.
;TurnAntiClockwise:
;Return

;TurnClockwise:
; Return

;StopTurning:
;Return
 

Gramps

Senior Member
Just a note
I have not had an opportunity to test this code with the hardware yet!
We greatly appreciate the help with the project even though we just don't have time to work on it now.
Hopefully later next week we will have a report.

Thanks Gramps
 

Gramps

Senior Member
Are we doing this right?

Code:
#Picaxe 28X2
Symbol DESIRED_POT = C.0
Symbol FEEDBACK_POT = C.1

Change To:

Code:
#Picaxe 28X2
Symbol DESIRED_POT = 17 ; C.5

Symbol FEEDBACK_POT = 4 ; C.3
PWM connected to C.2
Direction connected to C.4

We are getting CCW and CW movement, but the position of the pots does not effect the operation.
 
Last edited:

lbenson

Senior Member
The ADC channels look right: 17 for C.5 and 4 for C.3.

What do you get if you run
Symbol DESIRED_POT = 17 ; C.5
PAUSE 1000
READADC DESIRED_POT,b1
sertxd("On channel ",#DESIRED_POT, ", ADC value read = ",#b1,cr,lf)

If that works, put it in a DO loop and see if the value changes as expected when you turn the pot. Adjust the pause as necessary.
 

Gramps

Senior Member
Thanks Lance we'll try that.
Have not had any time at all lately to experiment.
put it in a DO loop
Success!
Pot is responding correctly in the terminal!
So next we check the feedback pot the same way. Correct?
Edit;
FEEDBACK pot is also working!!!!
 
Last edited:

Gramps

Senior Member
C.2 pwm pin is running the motor continuously. Pot has no effect on movement.
C.4 direction pin remains high even when the pot is moved through it's range. When it's disconnected, the direction reverses.
 

hippy

Senior Member
Please post your full code.

Does the motor turn clockwise or anticlockwise when C.4 is high ?

It could be that your pots are reversed compared to each other. Does the target pot voltage increase or decrease when you move it in a clockwise direction ? Does the feedback pot voltage increase or decrease as the motor turns clockwise.

Is the feedback pot connected to the motor ? If not it won't match most target settings and will continuously rotate.
 

Gramps

Senior Member
Is the feedback pot connected to the motor ?
We had to use a separate motor for testing because the one with the pot mounted on it has limited range.

your pots are reversed compared to each other.
Yup! That fixed it! Reversed the leads on one of the pots. Now working!
Please help us get a longer pause between direction changes. Thanks.

Code:
Code:
#Picaxe 28X2

Symbol DESIRED_POT     = 17; C.5
Symbol FEEDBACK_POT    = 4; C.3

; N = Vin * 1023 / Vpsu

Symbol FEEDBACK_TOP     = 757 ; 3.7V
Symbol FEEDBACK_MID     = 512 ; 2.5V
Symbol FEEDBACK_BOT     = 245 ; 1.2V

Symbol FEEDBACK_TOP_GAP = FEEDBACK_TOP - FEEDBACK_MID
Symbol FEEDBACK_BOT_GAP = FEEDBACK_MID - FEEDBACK_BOT

Symbol reserveW0        = w0 ; b1:b0
Symbol desiredPosition  = w1 ; b3:b2
Symbol currentPosition  = w2 ; b5:b4

#IfDef SIMULATING
  SerTxd("Testing", CR, LF )
  SerTxd("FEEDBACK_TOP     = ", #FEEDBACK_TOP, CR, LF )
  SerTxd("FEEDBACK_MID     = ", #FEEDBACK_MID, CR, LF )
  SerTxd("FEEDBACK_BOT     = ", #FEEDBACK_BOT, CR, LF )
  SerTxd("FEEDBACK_TOP_GAP = ", #FEEDBACK_TOP_GAP, CR, LF )
  SerTxd("FEEDBACK_BOT_GAP = ", #FEEDBACK_BOT_GAP, CR, LF )
  w0 = FEEDBACK_TOP : Gosub Test
  w0 = FEEDBACK_TOP - FEEDBACK_MID / 2 + FEEDBACK_MID : Gosub Test
  w0 = FEEDBACK_MID : Gosub Test
  w0 = FEEDBACK_MID - FEEDBACK_BOT / 2 + FEEDBACK_BOT : Gosub Test
  w0 = FEEDBACK_BOT : Gosub Test
  End
#EndIf
Do
  Gosub ReadDesiredPostion
  Gosub ReadCurrentPosition
  Select Case currentPosition
    Case  > desiredPosition : Gosub TurnAntiClockwise
    Case  < desiredPosition : Gosub TurnClockwise
    Else                    : Gosub StopTurning
  End Select
Loop

ReadDesiredPostion:
  ReadAdc10 DESIRED_POT, w0
CalcDesiredPosition:
  desiredPosition = w0
  Return

ReadCurrentPosition:
  ReadAdc10 FEEDBACK_POT, w0
CalcCurrentPosition:
  If w0 >= FEEDBACK_MID Then
    w0 = w0 - FEEDBACK_MID
    w0 = w0 * 128 / FEEDBACK_TOP_GAP * 4 Max 511
    w0 = w0 + 512
  Else
    w0 = FEEDBACK_MID - w0
    w0 = w0 * 128 / FEEDBACK_BOT_GAP * 4 Max 512
    w0 = 512 - w0
  End If
  currentPosition = w0
  Return

;TurnAntiClockwise:
; Return

;TurnClockwise:
; Return

;StopTurning:
  ;Return

#IfDef SIMULATING
Test:
  SerTxd("Current Nadc = ", #w0, TAB )
  Gosub CalcCurrentPosition
  SerTxd( #currentPosition, CR, LF )
  Return
#EndIf
Symbol  DIRECTION     = C.4
#Define CLOCKWISE     Low  DIRECTION
#Define ANTICLOCKWISE High DIRECTION

Symbol  MOTOR         = C.2
Symbol  MOTOR_PERIOD  = 100
Symbol  MAX_SPEED     = 300

Symbol  speed         = w3 ; b7:b6

TurnAntiClockwise:
  If currentPosition <= desiredPosition Then
    Gosub StopTurning
  Else If speed = 0 Then
    speed = MAX_SPEED
    ANTICLOCKWISE
    PwmOut MOTOR, MOTOR_PERIOD, speed
  Else
    PwmDuty MOTOR, speed
  End If
  Return

TurnClockwise:
  If currentPosition >= desiredPosition Then
    Gosub StopTurning
  Else If speed = 0 Then
    speed = MAX_SPEED
    CLOCKWISE
    PwmOut MOTOR, MOTOR_PERIOD, speed
  Else
    PwmDuty MOTOR, speed
  End If
  Return

StopTurning:
  PwmOut MOTOR, OFF
  speed = 0
  Return
[Code]
 

hippy

Senior Member
Please help us get a longer pause between direction changes. Thanks.
I think putting PAUSE commands after stopping the motor when turning should do the job ...
Putting the pause in the stop routine is probably a better choice ...
Rich (BB code):
Symbol PAUSE_MS = 2000 ; Millisecond delay when target reached
Rich (BB code):
StopTurning:
  PwmOut MOTOR, OFF
  If speed <> 0 Then
    Pause PAUSE_MS
  End If
  speed = 0
  Return
 
Last edited:

Gramps

Senior Member
Thanks for the stop routine.
The code works but not as we hoped. We wanted the DESIRED pot to cause the FEEDBACK pot to move only as it is adjusted. Then it should stop until the DESIRED pot is moved again.
Instead when bumping the DESIRED pot a little causes the FEEDBACK pot to travel to the limit of it's travel. Then turning the DESIRED pot the other way a little causes the FEEDBACK pot to travel to limit on the other end. I hope that it's clear what is happening.
 

hippy

Senior Member
when bumping the DESIRED pot a little causes the FEEDBACK pot to travel to the limit of it's travel.
It sounds like another inversion type issue or the software not matching what inversion there is.

Does the motor turn clockwise or anticlockwise when C.4 is high ?
Does the target pot voltage increase or decrease when you move it in a clockwise direction ?
Does the feedback pot voltage increase or decrease as the motor turns clockwise.
 

hippy

Senior Member
It seems the software is completely inverted to how the hardware is and it's quite possible that the pot mapping which was expecting an increasing voltage as the pot was turned clockwise fails to function correctly.

I would suggest adjusting both posts so they give an increasing voltage when turned clockwise, measuring the feedback pot voltages when the motor has turned the device near fully anti-clockwise, at its mid-point, and when near fully clockwise, updating FEEDBACK_BOT, FEEDBACK_MID and FEEDBACK_TOP as appropriate.

If the feedback pot has not reached its end stop value when the target pot is positioned to its extremes it will keep trying to turn the motor forever so it will be necessary to add some sort of end-stop dead zone.

Plus, alter the motor direction control to reflect reality -
Code:
#Define CLOCKWISE     High DIRECTION
#Define ANTICLOCKWISE Low  DIRECTION
That should get us to a firm foundation which is comprehensible; target pot voltage increases as turned clockwise, feedback pot voltage increases as motor turns clockwise, motor turns clockwise when the code instructs it to.
 

Gramps

Senior Member
Thanks hippy, that sounds right on . Earlier when we plotted the feedback pot in the terminal we noted that it was not going to full range.
Getting busy here again at the Rescue Mission so may not get back to this till next week.
 

Gramps

Senior Member
We reversed the motor direction in the code.

We measured the voltage and the terminal readout on both pots.
Desired Pot channel 17 (C.5)
Full ACW rotation, 0 volts, terminal reads 0
Full CW rotation, 4.83 volts, terminal reads 255

Feedback Pot channel 4 (C.3)
Full ACW rotation, 3.75 volts, terminal reads 200
Full CW rotation, .87 volts, terminal reads 48

Adjusting the Desired Pot is having some effect on the position of the motor-feedback pot but we can't see a relationship.
If we reset the the picaxe, the direction will reverse every time.
Edit:
It looks like one pot is backwards.
Edit:
We reversed the feedback pot. Now the Desired pot is changing the direction correctly but we must reset the chip between each change of direction.
 
Last edited:

hippy

Senior Member
Desired Pot channel 17 (C.5)
Full ACW rotation, 0 volts, terminal reads 0
Full CW rotation, 4.83 volts, terminal reads 255

Feedback Pot channel 4 (C.3)
Full ACW rotation, 3.75 volts, terminal reads 200
Full CW rotation, .87 volts, terminal reads 48
You need to rewire the feedback pot so the voltage increases from lowest when anti-clockwise to highest when clockwise.
 

Gramps

Senior Member
Yup did that!
We reversed the feedback pot. Now the Desired pot is changing the direction correctly but we must reset the chip between each change of direction.
 
Last edited:

Gramps

Senior Member
New readings:

Desired Pot channel 17 (C.5)
Full ACW rotation, 0 volts, terminal reads 0
Full CW rotation, 4.83 volts, terminal reads 255

Feedback Pot channel 4 (C.3)
Full ACW rotation, 1.02 volts, terminal reads 54
Full CW rotation, 3.92 volts, terminal reads 211
 

hippy

Senior Member
Now the Desired pot is changing the direction correctly but we must reset the chip between each change of direction.
Not sure why that would be. You can try this -
Code:
#Picaxe 28X2
#No_Data
#No_Table

Symbol DESIRED_POT      = 17 ; C.5 = ADC17
Symbol FEEDBACK_POT     =  4 ; C.3 = ADC4

; N = Vin * 1023 / Vpsu, Vpsu = 4.83V

Symbol FEEDBACK_TOP     = 830 ; 3.92V
Symbol FEEDBACK_MID     = 512 ; 2.47V
Symbol FEEDBACK_BOT     = 216 ; 1.02V

Symbol FEEDBACK_TOP_GAP = FEEDBACK_TOP - FEEDBACK_MID
Symbol FEEDBACK_BOT_GAP = FEEDBACK_MID - FEEDBACK_BOT

Symbol  DIRECTION       = C.4
#Define CLOCKWISE       High DIRECTION
#Define ANTICLOCKWISE   Low  DIRECTION

Symbol MOTOR            = C.2
Symbol MOTOR_PERIOD     = 100
Symbol MAX_SPEED        = 300

Symbol PAUSE_MS         = 2000 ; Millisecond delay when target reached

Symbol reserveW0        = w0 ; b1:b0
Symbol desiredPosition  = w1 ; b3:b2
Symbol currentPosition  = w2 ; b5:b4
Symbol speed            = w3 ; b7:b6

Do
  Gosub ReadDesiredPostion
  Gosub ReadCurrentPosition
  Select Case desiredPosition
    Case  > currentPosition : Gosub TurnClockwise
    Case  < currentPosition : Gosub TurnAntiClockwise
    Else                    : Gosub StopTurning
  End Select
Loop

ReadDesiredPostion:
  ReadAdc10 DESIRED_POT, w0
CalcDesiredPosition:
  desiredPosition = w0
  Return

ReadCurrentPosition:
  ReadAdc10 FEEDBACK_POT, w0
CalcCurrentPosition:
  If w0 >= FEEDBACK_MID Then
    w0 = w0 - FEEDBACK_MID
    w0 = w0 * 128 / FEEDBACK_TOP_GAP * 4 Max 511
    w0 = w0 + 512
  Else
    w0 = FEEDBACK_MID - w0
    w0 = w0 * 128 / FEEDBACK_BOT_GAP * 4 Max 512
    w0 = 512 - w0
  End If
  currentPosition = w0
  Return

TurnClockwise:
  Do
    If speed = 0 Then
      speed = MAX_SPEED
      CLOCKWISE
      PwmOut MOTOR, MOTOR_PERIOD, speed
    Else
      CLOCKWISE
      PwmDuty MOTOR, speed
    End If
    Gosub ReadDesiredPostion
    Gosub ReadCurrentPosition
  Loop Until currentPosition >= desiredPosition
  Gosub StopTurning
  Return

TurnAntiClockwise:
  Do
    If speed = 0 Then
      speed = MAX_SPEED
      ANTICLOCKWISE
      PwmOut MOTOR, MOTOR_PERIOD, speed
    Else
      ANTICLOCKWISE
      PwmDuty MOTOR, speed
    End If
    Gosub ReadDesiredPostion
    Gosub ReadCurrentPosition
  Loop Until currentPosition <= desiredPosition
  Gosub StopTurning
  Return

StopTurning:
  PwmOut MOTOR, MOTOR_PERIOD, 0
  If speed <> 0 Then
    Pause PAUSE_MS
  End If
  speed = 0
  Return
 

Gramps

Senior Member
Appears to work some but still erratic. we have switched back to testing with two Motors.
test video here
 
Last edited:
Top