 |
 |
| View previous topic :: View next topic |
| Author |
Message |
rovtech
Joined: 24 Sep 2006
Posts: 262
|
| Motor soft limits not behaving |
 Posted: Sun Feb 19, 2017 10:48 am |
 |
|
I have two routines that stop an underwater robotic arm at up or down limits and back it off slightly. The up limit works perfectly but the down limit stutters before stopping even though the two routines are identical.
The positioning of the motor is by a 10k pot mechanically coupled to the motor. The problem has nothing to do with backlash or noise.
The 16F1509 controls two motors and both do exactly the same thing. Proper PCBs and permanent construction are used. i.e. this is not a breadboard system.
The relevant routines are below and are in an endless polling loop. The arm movement is controlled by joysticks containing switches. DOWN movement of the motor causes the voltage and position value to increase, UP decreases the value, sorry for the confusion. I am using the adc value raw.
I have added ‘&&!at_arm_limit’ to the ‘if’ statements for troubleshooting to only allow the routine to be used once. It will reset if I let go of the joystick in which case the routine finishes backing off the limits (a second try). The top routine works 100% but the bottom one usually stops without backing up. Without this addition the arm gets there with some stuttering which told me it was getting stopped and not completing in the while loop. HOW CAN IT NOT COMPLETE THE WHILE LOOP? Perhaps I’m using the while incorrectly? A delay after the 'read_adc();' does not help.
| Code: | #define down_limit 80 // arm down limit
#define up_limit 30 // arm up limit
// stop if the arm has reached limits
if (arm_posn<up_limit&&!at_arm_limit) // if upper range limit exceeded
{
at_arm_limit = 1; // set flag in status
while (arm_posn<up_limit+2) // move off limit
{
arm_posn=read_adc();
move_down();
}
stop_arm(); // stop
}
if ((arm_posn>down_limit)&&(!at_arm_limit))
{
at_arm_limit = 1; // set flag in status
while (arm_posn>(down_limit-2)) // move off limit
{
move_up();
arm_posn=read_adc();
}
stop_arm(); // stop
}
|
There are identical routines for the second motor and they behave exactly the same, the up limit works 100% but the down limit usually fails.
I am adding the full program below. The PIC that controls these motors gets its instructions on I2C via an interrupt from a control console. If any of this was causing the problem then surely it would affect both limits, not just the one.
The software requirements are: Deploy, Retract, joystick interrupts deploy, deploy stops retract, joystick does not bother retract, motor will retract within the middle of deploy, motor stops at upper limit and backs off limit, motor stops at lower limit and backs off limit.
Is there a way to attach my flowchart as .pdf or .jpg here?
| Code: | //////////////////////////////////////////////////////////////////////////
// Upper Arm Section.C //
// Last modified: 18 Feb 2017 //
// A program to control the Upper Arm Section using I2C //
// Use 16F1509 and I2C at address 0x1C. --working-- //
// Set limits, retract, status, & deploy for UP/DOWN //
// Set limits, retract, status, & deploy for IN/OUT //
// Return status and display problems on console //
// Viewed from bracket side both motors rotate CW and the pot voltage //
// goes UP when MOVE IN or DOWN is selected //
// Status.7 Leak water detected in tube //
// Status.6 arm_jam arm motor jam //
// Status.5 at_arm_limit arm motor has reached a limit //
// Status.2 forearm_jam forearm motor jam //
// Status.1 at_forearm_limit forearm motor has reached a limit //
//////////////////////////////////////////////////////////////////////////
/* Pre-processor directives */
#include <16F1509.H>
#fuses INTRC_IO, NOCLKOUT, NOWDT, PUT, NOPROTECT, NOBROWNOUT
#device ADC = 8
#use delay (clock = 16 MHz) // PIC runs at 16 MHz
#byte portA = getenv("SFR:PORTA")
#byte portB = getenv("SFR:PORTB")
#byte portC = getenv("SFR:PORTC")
#use i2c (SLAVE, SCL=PIN_B6, SDA=PIN_B4, address=0x1C)
#define down_limit 80 // arm down limit 226
#define up_limit 30 // arm up limit
#define arp 31 // arm retract position
#define adp 55 // arm deploy position 150
#define in_limit 80 // forearm in limit 226
#define out_limit 30 // forearm out limit
#define frp 31 // forearm retract position
#define fdp 55 // forearm deploy position 150
#define Imax 130 // motor current limit
// Function Prototypes
void move_up(void); // move arm up
void move_down(void); // move arm down
void stop_arm(void); // stop arm movement
void move_in(void); // move arm in
void move_out(void); // move arm out
void stop_forearm(void); // stop IN/OUT movement
// global variables
char arm_ctrl = 0x00; // arm controls
char arm_posn = 127; // arm vertical position
char arm_status = 0; // arm status
char arm_reset = 0; // arm resets
char forearm_posn = 127; // forearm position
// Interrupt on I2C
#INT_SSP
void ssp_interrupt () // have an interrupt
{
char incoming, state; // variables
state = i2c_isr_state (); // get state
if (state < 0x80) // master is sending data
{
incoming = i2c_read (); // throw away device address if state = 0
if (state == 1) // first data received is arm ctrl
arm_ctrl = incoming;
if (state == 2) // second data received is arm resets
arm_reset = incoming;
}
if (state >= 0x80) // master is requesting data from slave
{
if (state==0x80)
i2c_write (arm_status); // send arm status
if (state==0x81)
i2c_write (arm_posn);
}
}
// ********** The main function **********
void main()
{
setup_oscillator (OSC_16MHZ);
// initialize port directions
set_tris_a (0xDB); // set Port A 11011011
set_tris_b (0x7F); // set Port B 01111111
set_tris_c (0x83); // set Port C 10000011
// declare variables
#bit retract = arm_ctrl.5 // 0 = deploy, 1 = retract
#bit arm_in = arm_ctrl.3 // arm IN
#bit arm_out = arm_ctrl.2 // arm OUT
#bit arm_up = arm_ctrl.1 // 1 = arm up
#bit arm_down = arm_ctrl.0 // 1 = arm down
#bit leak = arm_status.7 // water leak
#bit arm_jam = arm_status.6 // arm motor jam
#bit at_arm_limit = arm_status.5 // arm motor has reached a limit
#bit forearm_jam = arm_status.2 // forearm motor jam
#bit at_forearm_limit = arm_status.1 // forearm motor has reached a limit
char arm_cur = 0; // arm motor current
char forearm_cur = 0; // forearm motor current
short en_depl = 1; // shows deploy mode
short forearm_flag = 1; // shows deploy mode
// setup ADC
setup_adc (ADC_CLOCK_INTERNAL); // FRC clock, 2-6 us
setup_adc_ports (sAN3|sAN4|sAN5|sAN9|sAN11); // arm posn, forearm posn, leak,
// arm current, forearm current
// setup interrupts
enable_interrupts(INT_SSP);
enable_interrupts(GLOBAL);
// Initialize motor
stop_arm(); // stop UP/DOWN motor
stop_forearm(); // stop IN/OUT motor
retract = 1; // start at retract
// ********** main loop **********
while (1) // loop endlessly
{
// *****These routines all deal with the UP/DOWN arm motor*****
set_adc_channel (3); // points a/d at u/d posn
delay_ms(5); // wait
arm_posn = read_adc(); // read position
// Fold if retract is selected
if(retract&&!arm_jam) // retract selected
{
if (at_arm_limit)
en_depl = 1;
else
move_up(); // move to retract
}
// Deploy if selected and not already deployed
if(!retract&&!arm_jam) // deploy selected
{
if(en_depl) // do until flag is cleared
{
if((arm_posn<(adp-2))) // if posn < arm deploy position
move_down(); // rotate down or
if((arm_posn>(adp+2)))
move_up(); // up, as required
if (arm_posn>(adp-2)&&(arm_posn<(adp+2))) // until deployed
{
stop_arm(); // then stop and
en_depl = 0; // abort deploy
}
}
// **Check for instructions**
if ((arm_up||arm_down)&&(!at_arm_limit)) // if arm selected
{ // & not at limit
en_depl = 0; // abort deploy
if (arm_up && arm_posn>up_limit)
move_up(); // move arm up
if (arm_down && arm_posn<down_limit)
move_down(); // or down
}
// stop if the joystick is released
if ((!arm_up)&&(!arm_down)&&(!en_depl)) // if joystick released while
{ // arm is deployed
stop_arm(); // stop
at_arm_limit = 0; // and clear limit flag
}
} // end of deploy loop
// stop if the arm has reached limits
if (arm_posn<up_limit&&!at_arm_limit) // if upper range limit exceeded
{
at_arm_limit = 1; // set flag in status
while (arm_posn<up_limit+2) // move off limit
{
arm_posn=read_adc();
move_down();
}
stop_arm(); // stop
}
if ((arm_posn>down_limit)&&(!at_arm_limit))
{
at_arm_limit = 1; // set flag in status
while (arm_posn>(down_limit-2)) // move off limit
{
move_up();
arm_posn=read_adc();
}
stop_arm(); // stop
}
// Check arm motor current & stop if there is a jam
set_adc_channel (9); // points a/d at channel 9
delay_ms(5); // wait
arm_cur = read_adc(); // reads arm motor current
if (arm_cur > Imax) // if excess motor current
{
arm_jam = 1; // set status flag & stop
stop_arm();
}
// *****These routines all deal with the IN/OUT motor*****
set_adc_channel (4); // points a/d at in/out posn
delay_ms(5); // wait
forearm_posn = read_adc(); // read position
// Align if retract is selected
if(retract&&!forearm_jam) // if retract selected
{
if(forearm_posn<(frp-2)) // if at limit
move_in(); // move in or
if(forearm_posn>(frp+2))
move_out(); // out, as required
if (forearm_posn>(frp-2)&&(forearm_posn<(frp+2))) // until deployed
{
stop_forearm(); // then stop and
forearm_flag = 1; // enable deploy
}
}
// Deploy if selected and not already deployed
if(!retract&&!forearm_jam) // deploy selected
{
if(forearm_flag) // do until flag is cleared
{
if((forearm_posn<(fdp-2))) // move in or
move_in();
if((forearm_posn>(fdp+2))) // out, as required
move_out();
if (forearm_posn>(fdp-2)&&(forearm_posn<(fdp+2))) // until deployed
{
stop_forearm(); // then stop and
forearm_flag = 0; // abort deploy
}
}
// **Check for instructions**
if ((arm_out||arm_in)&&(!at_forearm_limit)) // if forearm selected
{ // and not at limit
forearm_flag = 0; // abort deploy
if (arm_out && forearm_posn>out_limit)
move_out(); // move forearm out
if (arm_in && forearm_posn<in_limit)
move_in(); // or in as required
}
// stop if the joystick is released
if ((!arm_out)&&(!arm_in)&&(!forearm_flag)) // if joystick released while
{ //arm is deployed
stop_forearm(); // stop
at_forearm_limit=0; // and clear limit flag
}
} // end of deploy loop
// stop if the forearm has reached limits
if (forearm_posn<out_limit) // if upper range limit exceeded
{
at_forearm_limit = 1; // set flag in status
while (forearm_posn<out_limit+5) // move off limit
{
move_in();
forearm_posn=read_adc();
}
stop_forearm(); // stop
}
if (forearm_posn>in_limit) // if lower range limit exceeded
{
at_forearm_limit = 1; // set flag in status
while (forearm_posn>in_limit-5) // move off limit
{
move_out();
forearm_posn=read_adc();
}
stop_forearm(); // stop forearm
}
// Check forearm motor current & stop if there is a jam
set_adc_channel (11); // points a/d at channel 11
delay_ms(5); // wait
forearm_cur = read_adc(); // reads forearm motor current
if (forearm_cur > Imax) // if motor current is high
{
stop_forearm(); // stop
forearm_jam = 1; // and set flag
}
// Check for a Leak
set_adc_channel (5); // points a/d at leak sensor
delay_ms(5); // wait
if (read_adc()<175) // if sensor pulling I/P down
leak = 1; // set flag in status
} // end of endless while loop
} // end of main function
// ***************functions********************************
// function 'move_up' moves the arm up
void move_up (void)
{
output_low (PIN_C6);
output_high (PIN_C4); // Set direction UP
output_high (PIN_C3); // enable L298
}
// function 'move_down' moves the arm down
void move_down (void)
{
output_high (PIN_C6);
output_low (PIN_C4); // Set direction DOWN
output_high (PIN_C3); // enable L298
}
// function 'stop_arm' stops the arm motors
void stop_arm (void)
{
output_low (PIN_C3); // disable L298
}
// function 'move_in' moves the forearm in
void move_out (void)
{
output_low (PIN_B7);
output_high (PIN_A5); // Set direction IN
output_high (PIN_A2); // enable L298
}
// function 'move_out' moves the forearm out
void move_in (void)
{
output_high (PIN_B7);
output_low (PIN_A5); // Set direction OUT
output_high (PIN_A2); // enable L298
}
// function 'stop_forearm' stops the forearm motor
void stop_forearm (void)
{
output_low (PIN_A2); // disable L298
}
// end |
|
|
 |
newguy
Joined: 24 Jun 2004
Posts: 1909
|
| Re: Motor soft limits not behaving |
| Posted: Sun Feb 19, 2017 10:58 am |
|
|
| rovtech wrote: | | Code: | #define down_limit 80 // arm down limit
#define up_limit 30 // arm up limit
// stop if the arm has reached limits
if (arm_posn<up_limit&&!at_arm_limit) // if upper range limit exceeded
{
at_arm_limit = 1; // set flag in status
while (arm_posn<up_limit+2) // what happens if you make this (up_limit+2), same as for down?
{
arm_posn=read_adc();
move_down();
}
stop_arm(); // stop
}
if ((arm_posn>down_limit)&&(!at_arm_limit))
{
at_arm_limit = 1; // set flag in status
while (arm_posn>(down_limit-2)) // move off limit
{
move_up();
arm_posn=read_adc();
}
stop_arm(); // stop
}
|
|
I've been bitten by NOT putting parenthesis in my if()s. Your up (which works) doesn't match your down code (which doesn't). What happens if you put the limit test for up in parenthesis, like you have for down? |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Sun Feb 19, 2017 11:32 am |
|
|
It stutters. You have a routine that stops the arm if there is over-current.
That could be the source of the stuttering.
| Quote: |
// Check arm motor current & stop if there is a jam
set_adc_channel (9); // points a/d at channel 9
delay_ms(5); // wait
arm_cur = read_adc(); // reads arm motor current
if (arm_cur > Imax) // if excess motor current
{
arm_jam = 1; // set status flag & stop
stop_arm();
}
|
Add a soft UART at 115K baud on an unused pin. Add a printf statement
to the over-current routine (or to the stop_arm() routine) that will display
a message when this occurs. Or add an LED to do this.
Or, if you think one routine is failing, put several printf's in there and
print out the variables. You'll probably see some anomaly.
Edit: Fixed a typo
Last edited by PCM programmer on Sun Feb 19, 2017 11:53 am; edited 1 time in total |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19549
|
|
| Posted: Sun Feb 19, 2017 11:47 am |
|
|
What happens if you reverse the order of the tests?. Shouldn't matter, but does mean there is slightly more delay reaching the 'down'. If it changes things then you have a pointer to the problem.
My guess would actually be that you are meeting a difference in the mechanical behaviour of the system. When you stop going down, it tends because of it's weight to overshoot, so has to move back further than it might appear, hence a jerk.... |
|
|
rovtech
Joined: 24 Sep 2006
Posts: 262
|
|
| Posted: Sun Feb 19, 2017 1:34 pm |
|
|
Thanks 'newguy'. The top one works the bottom one does not. This is why I put in extra brackets to see if this was the problem, but it is not.
Thanks 'PCM Programmer' yes I commented out the stop in the overcurrent stuff, and disabled anything else that may be to blame, no help. I put in delays here and there including after the 'read_adc(); which changed things but did not fix the problem. This led me to believe that the 'while' loop was not completing. I don't have an extra pin. The extra condition in the 'if' statement tells me that the 'while' statement is not completing and is exiting with the motor at the limit, not backed off. If I release the joystick it resets 'at_arm_limit' and the next time thru the motor backs off. Moving and holding the joystick 'down' causes the motor to move to the limit again but it does not back off, most of the time. It does work 1/3 of the time.
Thanks Ttelmah. Two motor, two position pots, same identical problem always on the down. Both motors are on the bench with nothing on the shafts but the belt to the position pot. I tried pressing on the belt to remove any backlash (not much) but it has no effect. If I change the backoff to 20 (while (arm_posn>(down_limit-20)) it does not help. I tried adding 'short motor out' and a delay in the stop function, no help. I just swapped the position of the two routines and the problem stays with the down limit. I agree it seems like a mechanical problem but on two systems? I wish I could post a picture and the flowchart.
How can the 'while' loop not complete, and why on the down limit only?
I was thinking of making the backoff a function and doing away with the 'while' function, but then I would not ever know what the problem is.
Is it OK to keep reading the adc in the while loop? A delay after it does not help and, besides, it works OK on the up limit.
I tried adding a stop and delay to reduce the current surge of sudden reverse. See below, it does not help.
| Code: | if ((arm_posn>down_limit)&&(!at_arm_limit))
{
at_arm_limit = 1; // set flag in status
stop_arm();
delay_ms(500);
while (arm_posn>(down_limit-10)) // move off limit
{
move_up();
arm_posn=read_adc();
}
stop_arm(); // stop
} |
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19549
|
|
| Posted: Sun Feb 19, 2017 2:06 pm |
|
|
I think the mechanical issue is inherent in the weight involved. Measure the current drawn when moving 'up' versus what it draws when going 'down'. Try attaching a microswitch, and having this disconnect the drive, then run it up to this, then down to this. I bet you are going to find it goes a very significant distance further from the switch point in the down direction, versus the up.
This is why servo systems don't just stop motors, but ramp the speed up and down. |
|
|
rovtech
Joined: 24 Sep 2006
Posts: 262
|
|
| Posted: Sun Feb 19, 2017 2:08 pm |
|
|
Adding a 50ms delay after starting the motor in the opposite direction seems to fix the problem. 10ms does not and 100ms might be safer.
When the motor stops at the limits it is with a clunk that is louder than when this fix changes direction, or at the other limit. When the while loop did complete without this delay there was also no clunk. Gearhead motor driven by L298 H-bridge from 12v and drawing <100mA.
I don't know why this works, or why the while loop does not complete without it.
I will 'scope the power supplies for spikes that may be causing problems but don't think I will find anything. Why would a delay make any difference?
| Code: | if ((arm_posn>down_limit)&&(!at_arm_limit))
{
while (arm_posn>(down_limit-2)) // move off limit
{
at_arm_limit = 1; // set flag in status
move_up();
delay_ms(50);
arm_posn=read_adc();
}
stop_arm(); // stop
} |
|
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Sun Feb 19, 2017 2:17 pm |
|
|
Here's a similar comment from a revision description in an old
project that I did around the year 2000:
| Quote: |
// Set the MOTOR_START_DELAY_IN_TICKS to 10 (was 0)
// to prevent back-emf from occurring when the truck changes
// direction. The back emf voltage tripped the Astrodyne power
// supply's over-voltage circuit and caused it to shut off. This
// ended the cycle prematurely. It occurred in some units
// and not others, due to variations in the over-voltage trip
// point in the power supplies. |
The ticks were 10 ms each, so the delay was 100 ms.
This was a 12v DC motor that drew about 6 amps. |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19549
|
|
| Posted: Sun Feb 19, 2017 2:28 pm |
|
|
Obviously because it allows the motor time to actually stop.
When you stop in the down direction, it is almost certainly taking longer to stop. |
|
|
rovtech
Joined: 24 Sep 2006
Posts: 262
|
|
| Posted: Sun Feb 19, 2017 2:30 pm |
|
|
Thanks Ttelmah, 270mA going up, 290mA going down, about 500mA (on DVM) when the direction changes but I will have to 'scope it with DSO to see what it really is. I did not think it was that high. The motor and L298 are powered from a 12v battery. The 5v for the PIC runs off the battery but does not handle the motor current. No load and no arm to lift at present but things will get worse when it is installed, especially in air.
Thanks PCM Programmer, it looks like this is where I'm heading. Time for the DSO. |
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19549
|
|
| Posted: Sun Feb 19, 2017 2:38 pm |
|
|
I was just going to add, that whatever you are seeing, you have to realise it may well get worse when the masses increase, but I see you have realised this. 
Question. How are you stopping the motor?. Do you just remove the drive, or do you brake the motor?. Turn on both the high side drives or both the low side drives. This will make the arm stop better, but more violently.... |
|
|
rovtech
Joined: 24 Sep 2006
Posts: 262
|
|
| Posted: Sun Feb 19, 2017 2:52 pm |
|
|
Doing this has a funny effect; the motor goes to the limit then sits in the while loop for about 40 seconds going tac-tac-tac... until it reaches the backoff point. It does not matter if the motor coasts a long way and the start current has to be less than the reverse current. It's getting stopped in the while loop.
I'm getting my 'scope.
| Code: | if ((arm_posn>down_limit)&&(!at_arm_limit))
{
stop_arm();
delay_ms(100);
while (arm_posn>(down_limit-2)) // move off limit
{
move_up();
arm_posn=read_adc();
}
stop_arm(); // stop
at_arm_limit = 1; // set flag in status
} |
|
|
|
Ttelmah
Joined: 11 Mar 2010
Posts: 19549
|
|
| Posted: Sun Feb 19, 2017 3:08 pm |
|
|
You need to realise that when it is coasting, it is acting as a generator. You have two voltage sources when you remove the drive. The first is the inductor in the motor coils, and the second it acting as a dynamo. The energy from both these has to go somewhere. The inductor can generate huge voltages for a moment (if not actually trapped, it'll rise till something breaks..). Now the voltages for these should be trapped by the diodes on the driver IC, but this can involve much higher instantaneous current than the normal drive.
Braking the motor is a better way to go for a servo type motion, but really needs something to limit the current. This is why normally you would ramp down at a defined rate, to a stop, then apply the brake.
With a 'non limited' brake, the motor is effectively 'bouncing'. The motor is stopping so fast that it is generating elastic behaviour in the attached mechanism... |
|
|
rovtech
Joined: 24 Sep 2006
Posts: 262
|
|
| Posted: Sun Feb 19, 2017 3:41 pm |
|
|
The 5v looks clean but there are 2 volt pulses on the 12v power supply by the time it gets to the board. It has very long (4ft) #28 wire and a 470uF capacitor at the PCB. I think the L298 H Bridge is getting stopped by the pulses. It should not be because the PIC is still driving the pins. However it has internal logic...
This also does not explain why the 50ms delay fix worked. Perhaps the L298 cannot take such fast commands as the while loop is giving, especially when the up direction is being made. I will read the specs.
The final version will have much shorter wires but I should think of using something heavier.
Darn, there goes my idea of feeding the wires up Polyflo tubing to make them waterproof and flexible. Anything bigger won't fit.
I can make the capacitor bigger as well.
I will do some mods to shorten the wires and re-test to see if this is really the problem.
Yes that fixed it! Time to think.
Thank you all for the help.
Note added:
I just checked the spec sheets and the current sense to the L298 is maximum -1v to +2.3v. I have been exceeding both limits and going to 2.5v. I think this is what has been tripping the motor off. The clunk is probably the L298 stopping the motor to protect itself. I need to do more tests. The motor current according to the sense resistor is 140mA going up, 180 mA going down, and just over an amp when reversing.
Last edited by rovtech on Sun Feb 19, 2017 4:19 pm; edited 1 time in total |
|
|
PCM programmer
Joined: 06 Sep 2003
Posts: 21708
|
|
| Posted: Sun Feb 19, 2017 3:46 pm |
|
|
| You're saying the wire is 28 AWG ? |
|
|
|
|
You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot vote in polls in this forum
|
Powered by phpBB © 2001, 2005 phpBB Group
|
FAQ 
Search
Register
Profile 
Log in to check your private messages 
Log in
