jcchurch13 / Mechaduino-Firmware

Hardware available here:
http://tropical-labs.com
Other
389 stars 232 forks source link

Only three different step phases in a four phase step sequence? - two are duplicate #36

Closed tarchive closed 3 years ago

tarchive commented 5 years ago

Hello,

I am sure that i am missing something here but why are there only 3 different phases when stepping the motor?

Since i am still doing some initial testing, i am current running this on my Arduino Zero. I discovered when probing pins 5, 6 ,7 ,8 and manually commanding steps from serial console, i would see the following pattern: (Excuse my crude table)

            D8      D7       D5      D6
           IN_1    IN_2     IN_3    IN_4
Phase 1:    L       H        L       H
Phase 2:    L       H        H       L
Phase 3:    H       L        L       H 
Phase 4:    L       H        L       H 

As you can see, phase 1 and 4 are the same. I would have expected to see H L H L in one of them instead. Digging, I think i found the responsible code in the utils.cpp and the way it was written makes this seem intentional.

So either i am having a brain fart today or something is going on here.

Thanks

Quas7 commented 4 years ago

Hi,

I just add the A4954 pwm motor driver datasheet and the the PWM truth table on page 4 here. http://elecanisms.olin.edu/handouts/2.3_A4954-Datasheet.pdf

I think, the truth table format is a bit crowded as table is 2in1.

tarchive commented 4 years ago

Thanks for the reply Quas7 You are make me rethink this again because i did not test the pins with my oscilloscope to see the PWM duty though i don't think that's the issue. In my test i was using an Arduino Zero and LEDS on the output digital IO step pins labeled above. No A4954 driver was used so i did not actually test the drivers output either. However, i don't think this matters because the driver is just two full bridge drivers that relies on 4 IN signals to sequence the steps. The PWM is for decay control and does not affect the step sequence. In the table on page 4 it only shows IN3 and IN4 because its a truth table of a DC motor. Here in this project for one bipolar stepper we need IN1 IN2 IN3 and IN4 so that truth table only illustrates half the picture of whats going on.

Because i am observing this three phase pattern directly from the micro controller, I was thinking there is something seriously wrong with my setup. Though i was using vanilla code directly from the master branch. The really concerning part for me is that by manually telling the micro controller to step through the phases, i can see it pause where there is a duplicate phase. The micro controller knows there are 4 phases, its just duplicating one for some reason.

Since this post, i have already disassembled my test board and went with another Coretex-M0 compatible closed loop stepper driver project that has worked..alright for me.

I would still love to get to the bottom of this so i do appreciate all comments or explanations no matter how long since the original post. Thank you.

Varghesemela commented 3 years ago

I am facing the same issue, i uncommented the "debugging phase voltage" snippet at Line 112 in Utils.cpp. The serial monitor readings for the v_coil_A and v_coil_B are as below for single step sequence: 0, 37 37, 0 0, -37 -37, 0 And i also managed to get the output readings per hase for every 4 sequence which is as below: 0110 0101 1010 0110 As i can see one sequence gets repeated which is similar to this issue raised. Please let me know if anyone has an explanation.

Quas7 commented 3 years ago

There is some additional explanation, how this works for the Mechkaduino re-works on STM32 basis here: https://github.com/makerbase-mks/MKS-SERVO42B/issues/8 Maybe this helps.

jcchurch13 commented 3 years ago

Let me see if I can clarify what is happening here: IN_1 and IN_2 set the direction and decay mode for Phase A, and IN_3 and IN_4 set the direction and decay mode for Phase B. (H,L) is forward and (L,H) is reverse. However, the phase current is set by the V_REF pins on the A4954.

The logic in the firmware is to set the direction pins as follows:

` v_coil_A = ((effort sin_coil_A) / 1024); v_coil_B = ((effort sin_coil_B) / 1024);

analogFastWrite(VREF_1, abs(v_coil_A)); analogFastWrite(VREF_2, abs(v_coil_B));

if (v_coil_A >= 0) { IN_2_HIGH(); //REG_PORT_OUTSET0 = PORT_PA21; //write IN_2 HIGH IN_1_LOW(); //REG_PORT_OUTCLR0 = PORT_PA06; //write IN_1 LOW } else { IN_2_LOW(); //REG_PORT_OUTCLR0 = PORT_PA21; //write IN_2 LOW IN_1_HIGH(); //REG_PORT_OUTSET0 = PORT_PA06; //write IN_1 HIGH }

if (v_coil_B >= 0) { IN_4_HIGH(); //REG_PORT_OUTSET0 = PORT_PA20; //write IN_4 HIGH IN_3_LOW(); //REG_PORT_OUTCLR0 = PORT_PA15; //write IN_3 LOW } else { IN_4_LOW(); //REG_PORT_OUTCLR0 = PORT_PA20; //write IN_4 LOW IN_3_HIGH(); //REG_PORT_OUTSET0 = PORT_PA15; //write IN_3 HIGH }

}` When the Mechaduino performs single steps, it turns one phase on at a time in either forward or reverse.

The asymmetry arises due to the case when v_coil_X is 0. When you are commanding 0 current, it should not really matter whether you are in "forward" (H,L), or "reverse" (L,H), because that phase is effectively off.

This is all handled by the output function in Utils.cpp, which handles sinusoidal commutation of the motor phases. This is used by both the closed loop modes and open loop stepping. If this logic were faulty, the closed loop sinusoidal commutation would not work properly. For open loop stepping excitation angle is just stepped by 90 electrical degrees. In these cases you get v_coil_X set to exactly 0 on one phase at a time. The logic above determines the IN_X "direction" by checking if v_coil_X is >= 0. Based on this logic you get the following step pattern:

v_coil_A, v_coil_B: 37,0 IN_1, 2, 3, 4: 0,1,0,1

v_coil_A, v_coil_B: 0,-37 IN_1, 2, 3, 4: 0,1,1,0

v_coil_A, v_coil_B: -37,0 IN_1, 2, 3, 4: 1,0,0,1

v_coil_A, v_coil_B: 0,37 IN_1, 2, 3, 4: 0,1,0,1

As a test, I tried adding a small offset to the step angle to avoid the case where v_coil_X = 0. The results give four distinct IN_X codes, but there is no noticeable difference in operation:

v_coil_A, v_coil_B: 36,-1 IN_1, 2, 3, 4: 0,1,1,0

v_coil_A, v_coil_B: -1,-36 IN_1, 2, 3, 4: 1,0,1,0

v_coil_A, v_coil_B: -36,1 IN_1, 2, 3, 4: 1,0,0,1

v_coil_A, v_coil_B: 1,36 IN_1, 2, 3, 4: 0,1,0,1

I agree, it does look odd at first glance, but I don't think there is really an issue here unless someone is seeing unexpected behavior in the phase waveforms.