Balancing robot for dummies

Hi gbm,
I like your bot, thanks for sharing your project
I have tried using servo motors without success
My bot was definitly too heavy (to much inertia) for the small servos
Servos are slow (60 RPM), large wheels is a plus to get an acceptable linear speed
Please let me have a link to the servos you used

Your write up regarding PID looks pretty interesting, but Google translate gives sometime funny results ::slight_smile:

Special note for other Balancing Robot makers
links to your beloved creations are welcomed in this thread :slight_smile:

** Balancing robot for dummies **

Part three: Kalman filtering

6 - So... where are we now ??

As a reminder, here is the pseudo code for the balancing process:

loop                                        (fixed time loop 10ms = 100Hz)
- sensors aquisition, smoothing and zeroing
- Acc angle (quids) and Gyro rate (quids per second) calculation and scaling
- Acc and Gyro data fusing though Kalman algorithm                          << here we are
- PWM calculation using a PID control algorithm
- PWM feeding to DC motors
endLoop

7 - Data fusing

So, what do we get sofar ?? a noisy Acc angle and a drifting Gyro rate of rotation
Let's fuse them !!!
I have tried 2 filtering technologies:

When rotating the board, make sure that GYRO_rate has the right sign:
GYRO_rate should be positive when rotating board toward +256 Quids (+180°)
and negative when rotating board toward -256 Quids (-180°)
This is another pitfall (been there, done that ;))

The smoothing effect is quite impressive and is best viewed in graphing packages such as Processing or LabView

Watch it here P1020588 - YouTube?
The red line is the raw ACC angle, the blue one is the filtered Signal

This is the updated code including Kalman filter:

// Main module   K_bot angle    angles in Quids, 10 bit ADC -----------------------------
// 7 - Data fusing, Kalman filter

// Installation:
// Create "Kalman" and "Sensors" tabs
// Cut and paste the 2 modules in their respective tab
// Save as "Kas_bot_angle"

#include <math.h>

#define   GYR_Y                 0                              // Gyro Y (IMU pin #4)
#define   ACC_Z                 1                              // Acc  Z (IMU pin #7)
#define   ACC_X                 2                              // Acc  X (IMU pin #9)

int   STD_LOOP_TIME  =          9;             

int sensorValue[3]  = { 0, 0, 0};
int sensorZero[3]  = { 0, 0, 0 }; 
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
int actAngle;
int ACC_angle;
int GYRO_rate;

void setup() {
  analogReference(EXTERNAL);                                            // Aref 3.3V
  Serial.begin(115200);
  calibrateSensors();
}

void loop() {
  
// ********************* Sensor aquisition & filtering *******************
  updateSensors();
  ACC_angle = getAccAngle();                                           // in Quids
  GYRO_rate = getGyroRate();                                           // in Quids/seconds
  actAngle = kalmanCalculate(ACC_angle, GYRO_rate, lastLoopTime);      // calculate filtered Angle

// ********************* print Debug info *************************************
  serialOut_labView();

// *********************** loop timing control **************************
  lastLoopUsefulTime = millis()-loopStartTime;
  if(lastLoopUsefulTime<STD_LOOP_TIME)         delay(STD_LOOP_TIME-lastLoopUsefulTime);
  lastLoopTime = millis() - loopStartTime;
  loopStartTime = millis();
}

void serialOut_labView() {  
  Serial.print(actAngle + 512);        Serial.print(",");         // in Quids (0-512-1024)
  Serial.print(ACC_angle + 512);       Serial.print("\n");
}


// --- Kalman filter module  ----------------------------------------------------------------------------

    float Q_angle  =  0.001; //0.001
    float Q_gyro   =  0.003;  //0.003
    float R_angle  =  0.03;  //0.03

    float x_angle = 0;
    float x_bias = 0;
    float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;      
    float dt, y, S;
    float K_0, K_1;

  float kalmanCalculate(float newAngle, float newRate,int looptime) {
    dt = float(looptime)/1000;                                    
    x_angle += dt * (newRate - x_bias);
    P_00 +=  - dt * (P_10 + P_01) + Q_angle * dt;
    P_01 +=  - dt * P_11;
    P_10 +=  - dt * P_11;
    P_11 +=  + Q_gyro * dt;
    
    y = newAngle - x_angle;
    S = P_00 + R_angle;
    K_0 = P_00 / S;
    K_1 = P_10 / S;
    
    x_angle +=  K_0 * y;
    x_bias  +=  K_1 * y;
    P_00 -= K_0 * P_00;
    P_01 -= K_0 * P_01;
    P_10 -= K_1 * P_00;
    P_11 -= K_1 * P_01;
    
    return x_angle;
  }
 
// --- Sensors Module ------------------------------------------------------------------------------------

void calibrateSensors() {                                       // Set zero sensor values
  long v;
  for(int n=0; n<3; n++) {
    v = 0;
    for(int i=0; i<50; i++)       v += readSensor(n);
    sensorZero[n] = v/50;
  }                                                            // (618 - 413)/2 = 102.5    330/3.3 = x/1024
  sensorZero[ACC_Z] -= 100;    //102;                          // Sensor: horizontal, upward
}

void updateSensors() {                                         // data acquisition
  long v;
  for(int n=0; n<3; n++) {
    v = 0;
    for(int i=0; i<5; i++)       v += readSensor(n);
    sensorValue[n] = v/5 - sensorZero[n];
  }
}

int readSensor(int channel) {
  return (analogRead(channel));
}

int getGyroRate() {                                             // ARef=3.3V, Gyro sensitivity=2mV/(deg/sec)
  return int(sensorValue[GYR_Y] * 4.583333333);                 // in quid/sec:(1024/360)/1024 * 3.3/0.002)
}

int getAccAngle() {                      
  return arctan2(-sensorValue[ACC_Z], -sensorValue[ACC_X]) + 256;    // in Quid: 1024/(2*PI))
}

int arctan2(int y, int x) {                                    // http://www.dspguru.com/comp.dsp/tricks/alg/fxdatan2.htm
   int coeff_1 = 128;                                          // angle in Quids (1024 Quids=360°) <<<<<<<<<<<<<<
   int coeff_2 = 3*coeff_1;
   float abs_y = abs(y)+1e-10;                  
   float r, angle;
   
   if (x >= 0) {
     r = (x - abs_y) / (x + abs_y);
     angle = coeff_1 - coeff_1 * r;
   }  else {
     r = (x + abs_y) / (abs_y - x);
     angle = coeff_2 - coeff_1 * r;
   }
   if (y < 0)      return int(-angle);                   
   else            return int(angle);
}

We now have a nice filtered value reflecting the position of the bot:
Negative value when leaning forward
Positive value when forward
Zero when vertical, at equilibrium

Most of the sensitive job is done
Again, don't go further if you don't get an accurate, stable reading
This will save you a lot of frustration :wink:

Next part: PID Algorithm & (hopefully) DC motor control

This is a nice project thank you for sharing it, I am starting with my project and I will need your help :), I just order the parts.

Thank in advance

Thanks osiwb123 for the kind words
If you are still able to perform this operation, please remove the Poll items you placed at the begining of this thread.
Please start a new thread for this pool :wink:

Hey, haven't replied for a while. I found the issue with my gyro angle, but now my bot has been suffering from drifting... I have essentially gone back to the beginning...

A sad attempt :frowning:

Hi Gibby623

Your boot does balance and only need some tuning

void getGyroAlpha()
{
  rollRate = ((rollADC - rollADC_offset)*Aref/1024)/0.0091;
  /*if (rollRate < .5)
   {
   rollRate = 0;
   }*/
  gyro_alpha = prev_gyro_alpha + rollRate*0.011;
  prev_gyro_alpha = gyro_alpha;
}

Did you introduced a dead band as per above ??
I tried adding dead band and got overshooting as per your video

PID tuning
You may have inadequate derivative action (not enough damping)
Try modifying "D", also experiment negative values

Finally, please share your code, I will load it in my bot implementation

I will put up code soon, I have just re-written the whole code for a third time in 2 days!

And yes there is a dead band... not good?

http://www.youtube.com/watch?v=wLFRkj4HIG0

Your bot is becoming quite stable :slight_smile:

And yes there is a dead band... not good?

Try removing dead band and see for yourself (YMMV)
Also fine tune PID parameters. To avoid reloading code each time, parameters should be modified from the PC, using the serial monitor

I will put up code soon, I have just re-written the whole code for a third time in 2 days!

as soon as you post it, Iwill try it with my bot design

hi there , I have trouble searching for motors like yours. what are the factors that led you to choose this motor? I need to find a substitute but I do not know what to look out for. and they are rather expensive. :frowning:


Pros:

  • powerfull (110 oz.in)
  • solid design (metal gears)
  • fast (350RPM, don't settle for less than 200RPM)
  • integrated, efficient encoders (464 counts per wheel rotation)
  • acceptable price (40 bucks, including encoder)

Cons:

  • noticiable backlash (as with all spur gearmotors)

Planetary gearboxes are supposed to have no backlash
I have still to identify a planetary geared equivalent motor (speed, power, encoder)
any links ???

I am currently integrating encoders data in order to reduce forward/backward drift

Wandering around for new ideas, I came across this one:

I have yet to see a more stable bot :o :o
Lets hope this guy will open a blog soon for documenting his project
Congratulation beautifulsmall :slight_smile:

** Balancing robot for dummies **

Part four: PID control

image

Forewords: Control strategy is a whole univers, let's keep it simple :wink:

8 - Possible control strategies

error = Actual value - Set point

  • ON/OFF control
    This strategy in used for electric heaters with mechanical thermostat
pseudo code

if actTemp < SetPoint      Power = ON
else                  Power = OFF

Code for a basic (static) balancing robot
Set point = 0
error = actAngle

pseudo code

If actAngle = 0       Motors stop
else if actAngle <0      Motor full forward (PWM 256)
else if actAngle >0      Motor full backward (PWM 256)

Don't try it, it just doesn't work; let's be more sophisticated.

  • PID control (Proportional Integral Derivative)
    This control is the sum of 3 actions

Proportional action:
The proportional term makes a change to the output that is proportional to the current error value.
pTerm = Kp * error
A high value will cause a hard counter reaction to a change in attitude.
Above a certain value the system will start to oscillate, the ideal spot is just below this point.
You cannot ballance a robot with a proportional value of 0.
Proportional control only is much better than pure ON/OFF, but the Bot will keep oscillating and finally fall on the floor

Derivative action:
This action is proportional to the error variation
With a fixed time loop, time may be omited:

dTerm = Kd * (error - last_error);                            
  last_error = error;

The Derivative portion calculates how fast a change in angle occurs.
With ballancing bot, these values are usually used as a damping factor, meaning negative signs in the formula.
Without the derivative action, there is no damping to dissipate the energy of the system.
With only proportional control, any input to the system will cause an undamped oscillation that will continue for ever.
If you are an electrical engineer, think of how an RLC circuit differs from an LC circuit
The derivative term works the fastest, because it detects change in rate before even a significant error appears.

Integral action:

integrated_error += error;                                       
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);

GUARD_GAIN sets up a limit for iTerm.

The integral term allows a precise landing at setpoint, it is known as "steady state error".
It is the only term that keeps changing no matter how small the error remains.
You're basically telling the controller, "if you aren't quite getting to angle, keep increasing PWM over a period of time until you get there".

PID control is the sum of those 3 actions

int updatePid(int targetPosition, int currentPosition)   {
  int error = targetPosition - currentPosition; 
  pTerm = Kp * error;
  integrated_error += error;                                       
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = Kd * (error - last_error);                            
  last_error = error;
  return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}

The best PID analogy I found is the spring car suspension:

image

kp is the strength of the spring.
kd is the amount of damping needed to stop oscillations due to kp.

Proportional reacts to a measure of the present error.
Derivative reacts as a prediction of the future error,
Integral adjustment is based on past errors

PID tuning:
For manual tuning, set all three constants to zero, then turn up Kp until oscillation occurs.
Then turn up Kd until oscillation disappears. Adjust Kd until the system is critically damped, i.e. there's no overshoot.
Then increase Ki until the steady-state error goes to zero in a reasonable time.

Further readings:

http://www.jashaw.com/pid/tutorial/

Next part: DC motor control and final complete code (Version 1)

first post in Arduino,may well be teaching granny to suck eggs, been making a 2-wheel balancing robot for 12months now. earwig robot on youtube, The BMA180, arctan2, combined with the ADXR150 gyro(10-bit) gives a 1/16 of a degree resolution. Nice.complementary filter. After Hours spent tuning pid values the best solution I found was to have a timer giving a step change to the motor PID, the step level was interactive as were the pid values, this way you can see whare the Buzz and unstable parameters are. The balance equation,
vert_error+base_vel+body_vel+position_error.
if vert error is initially off, the robot drifts until the pos error cancels out and the bot oscillates around a + or -pos offset , this can be used to calibrate the true vertical.
trying to map now and getting lost.
i wouldn't be this far on without the help of others :o

Hi beautifulsmall, welcome to this forum :slight_smile: :slight_smile:

As already mentioned, you built the more stable Bot I have ever seen (I saw a lot those last 2 years ;))

I am really impressed by the lean back motion when pushed

My Version 1.0 KasBot doesn't yet take advantage of the encoder wheels;
It is stable and can balance forever, however it may drift forward/backward.
This is perfectly normal as the bot is not yet position aware
The idea is to inject encoder data in the PID loop:

KasBot V2 alpha

int updatePid(int targetPosition, int currentPosition)
  {
    int error = targetPosition - currentPosition; 
    pTerm = Kp * error;                             // error: angle
    integrated_error += error;                                       
    iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
    dTerm = Kd * (error - last_error);                            
    last_error = error;
    xpTerm = K2p * count;                               // count: encoder tics 
    xdTerm = K2d * (count - last_count);                          
    last_count = count;
    return -constrain(K*(pTerm + iTerm + dTerm + xpTerm + xdTerm), -255, 255);
  }

For encoders, I use speed proportional (K2p) and derivative (K2d), no integral
It does help somehow, forward/backward drift is reduced, but I am still far from what you achieved.

vert_error+base_vel+body_vel+position_errorNot sure to understand :-?
vert_error = angle
base_vel = angle (derivative)
body_vel = encoder (derivative)
position_error = encoder
please confirm

Please let us have additional details

  • forward/backward drift suppession
  • lean back

finally, please post relevant code or speudo code

Now I have gotten the parts and will start building a jig to be able to test the parts.

I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?

More info about the project:
http://www.x-firm.com/?cat=6

I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?

Could someone please explain me why the gyro should be in the centre of rotation. As far is I understand gyro measures angular velocity, which is exactly the same in every point of a rigid body.

I think the average acceleration would be the same in every point of the robot. When the robot starts leaning to some direction the upper part of the robot accelerates slowly and when it corrects the position the lower part accelerates rapidly.

I have started thinking about how to build the robot and especialy where to place the accelerometer and the gyro,

What I understand I need to have the gyro X-axis in the rotation centre of the wheels but how is it with the accelerometer. when its reading acceleration couldn't I place it where ever I want on the robot?

Could someone please explain me why the gyro should be in the centre of rotation. As far is I understand gyro measures angular velocity, which is exactly the same in every point of a rigid body.

I think the average acceleration would be the same in every point of the robot. When the robot starts leaning to some direction the upper part of the robot accelerates slowly and when it corrects the position the lower part accelerates rapidly.

Interesting question ::slight_smile:

I tend to agree with TJL for Gyros
Gyros sense rotation only, not acceleration.
Rotation is a "system wide" variable for a rigid system
They can be placed anywhere you want, even at the very top of the bot

I have a different analysis for accelerometers
Accelerometers measure both static acceleration (gravity) and dynamic acceleration (movement). They are also very noisy.
Static acceleration is system wide, dynamic acceleration (as speed) is not
As we are interested only in gravity measurement, we want dynamic acceleration as low as possible.
The best place for reducing rotational acceleration is near the the motor axis.

Others may jump in and correct me if I am wrong
For those who build or already built a bot, where did you decide to place the sensors ??

@Patrik: I went on your blog, nice start!
Keep feeding it and report here your baby's first steps :slight_smile:

Dear Readers,

This is my understanding of the sensor's placement for a balancing system, the ideas are off the top of my head and should not be considered right unless you feel they are correct or can be proven. Feel free to correct me, I am writing my Thesis at the moment and any input would be encouraged. Without further ado, my answer.

I tend to agree with TJL for Gyros
Gyros sense rotation only, not acceleration.
Rotation is a "system wide" variable for a rigid system
They can be placed anywhere you want, even at the very top of the bot

I agree Kas. The basic calculation to vary between translational and rotational movement is v = wr, where the translational velocity (v) can be found by multiplying the angular velocity (w) by the radius it is rotating (r).

As the gyroscope a.k.a angular rate sensor is able to read only rotational velocities, it may be placed anywhere on the robot, assuming that the sensing axis is parallel to the axis of rotation of the mechanical system.

Accelerometers measure both static acceleration (gravity) and dynamic acceleration (movement). They are also very noisy.
Static acceleration is system wide, dynamic acceleration (as speed) is not
As we are interested only in gravity measurement, we want dynamic acceleration as low as possible.
The best place for reducing rotational acceleration is near the the motor axis.

I partly agree with this. Accelerometers sense, as the name suggests, the acceleration along the corresponding sensing axis. However, I stray slightly from your definition Kas.

Your definition of dynamic acceleration seems more like the rate of change of acceleration, which, if anyone is interested it is known as a Jerk. I understand what you were trying to say though!

However, back to the point. In terms of a static system, the only acceleration the accelerometers will sense will the the earth's gravitational pull referenced to each sensing axis. This, for a balancing robot, is what you require from the sensor. However, the situation is complicated when the mechanical system also has translational acceleration. The Accelerometer sense translational accelerations, not angular acceleration. Returning back the to previous equation. As the rate of the angular velocity changes (i.e. angular acceleration) so does the translational acceleration of a particular point of the mechanical system, proportional to the radius of rotation.

So, what does this all mean? placing a gyroscope (theoretically) should be ok to place anywhere, paying close attention to the sensing axes.

Placing the accelerometer as close to the rotating axis is best. This can be proved considering if you placed the accelerometer precisely on the axis ie r = 0. Therefore the translational acceleration of the system will contribute wr = wx0 = 0m/s/s.

Does everyone agree? Too complicated? INCORRECT?

Regards!

P.S. Mentioned previously, I am writing my Thesis and also sitting exams therefore have had no time to program, however my more recent attempts of balancing can be seen here
-> http://www.youtube.com/watch?v=lRK9mlc0iD0 (moving forward for a given time stopping while balanced)
and
-> http://www.youtube.com/watch?v=wLFRkj4HIG0 (balancing, very unsteady!)

Regards, AGAIN! ;D

Hi, for months i just used the vertical angle , or its error from vertical to drive the PID data for the wheels. despite my best efforts the robot would drift and when pushed would catch up with vertical then stop the wheels but the upper body momentum would carry on giving a jerky start stop motion. so i tried adding in (or subtract ) the body velocity, calculated from the change in angle /dt , wheel velocity must be added to body velocity for a true body velocity. I had it all in different places but it was the same as N-bot's equation which I finally used in N-bots form.
Im not a SW engineer, code is messy.
I average the pwm, just by 2 but its jerky without. Code is for aDsPic.

setting Kpos=0, I tuned Kbody and Kbase which gives the lean back when pushed, also when moving between two locations the wheels move away ,changing the body angle towards the target, the wheels then change direction and the whole robot moves towards the target. when it reaches the target position the wheels speed up and overshoot to slow the body. Ill try and film this. The position error stops drift by offsetting the target vertical (which is incorrect if the robot drifts, and changes every time you breath), the robot will drift until the position offset gives a -ve motor drive when the robot will move back toward the zero position (not to the zero), but will oscillate around some offset.
As for gyro positioning, the gyro is a mems device, A lump in a void on springs, accel will affect it although it is compensated out ??. yes both my sensors are in completely the wrong place

as you can see the Kalman filter is on commented out, I just don't have a spare lifetime to understand it (or tune it) and the comp filter works.

going to try cheap motors next with ball mouse encoders. :slight_smile: :slight_smile:

// ***************** gyro read and average *************
a_angle=BMA180_accel_read();

gyro_delta=gyro_read(gyro_null);
ga=ga+(gyro_delta*dt);

comp_angle = (0.997 * (comp_angle +(gyro_delta * dt)))+(0.003 * (a_angle));   // complimentary filter
angle[0]=(16*comp_angle)-vertical;  //  adjust for 1/16 degree , and vertical offset

//ars_predict(&filter_roll, gyro_delta, dt);    // Kalman predict

//K_angle = ars_update(&filter_roll, a_angle);        // Kalman update + result (angle)
     


//************ GET WHEEL VELOCITY************
velocity_read(dt,&M1_vel,&M2_vel,&map_x,&map_y,&heading,&delta_pos);  // returns velocity, updated x,y and current heading
base_vel=(M1_vel/2)+(M2_vel/2);

//************ GET BODY VELOCITY**********
body_vel=body_velocity(comp_angle,M1_vel,M2_vel,dt,height);

// **********GET POSITION *************
// use a local x which is referenced to the wheels only and not any map.use this to correct for drift
// and to calibrate the balance vertical this local X can be negative where distance to goal cannot
//get_heading(map_x,map_y,req_x,req_y,&req_heading,&distance);

//if (mode==0)req_heading=0;

heading_error=heading-req_heading;



local_x=local_x+delta_pos; //use x,heading should already be correct
pid_pos_error=position_PID(local_x,Pp,Pi,Pd,dt);


//if ((heading_error>5)||(heading_error<-5)) distance=0;   //  get correct heading before

//pid_pos_error=-position_PID(distance,Pp,Pi,Pd,dt);  // for mode2 use distance

//******************* Heading PID ************************//


PID_heading=heading_PID(heading_error,Hp,Hi,Hd,dt);


// *** DO BALANCE PID **********************


c_angle=(angle[0]+angle[1]+angle[2]+angle[3])/4;
angle[3]=angle[2];
angle[2]=angle[1];
angle[1]=angle[0];


//error= sum of angle, body_velocity, wheel rate, position
error=((c_angle))+((Kbody*body_vel)/32)+((Kbase*base_vel)/32)+((Kpos*pid_pos_err
or)/16); // use 1/16 of a degree as standard    COMPLEMENTARY FILTER


balance_error=balance_PID(error,Kp,Ki,Kd,dt);   // get actual balance PID error

motor_PID(balance_error,PID_heading,Mp,Mi,Md,M1_vel,M2_vel,&M1_pwm,&M2_pwm,dt);


av3_pwm1=av2_pwm1;
av2_pwm1=av1_pwm1;
av1_pwm1=av0_pwm1;
av0_pwm1=M1_pwm;
M1_pwm=(av0_pwm1+av1_pwm1)/2;   //+av2_pwm1+av3_pwm1)/4;

av3_pwm2=av2_pwm2;
av2_pwm2=av1_pwm2;
av1_pwm2=av0_pwm2;
av0_pwm2=M2_pwm;
M2_pwm=(av0_pwm2+av1_pwm2)/2;  //+av2_pwm2+av3_pwm2)/4;





M1_dir=0;
if (M1_pwm >1) M1_dir=2;     // forward ?
if (M1_pwm <-1) M1_dir=1;     // forward ?

M2_dir=0;
if (M2_pwm >1) M2_dir=2;     // forward ?
if (M2_pwm <-1) M2_dir=1;     // forward ?

M1_pwm=abs(M1_pwm); // +deadzone;
M2_pwm=abs(M2_pwm); // +deadzone;
if (M1_pwm>200) M1_pwm=200;
if (M2_pwm>200) M2_pwm=200;

if (abs(error)>720) // stop motors if fallen over > 45 degrees
{      M1_pwm=0;
     M2_pwm=0;
}
//if (int_seconds<5)
if (motor_en & 1)
{
M1_dir=2;
M2_dir=2;
M1_pwm=0;  // test step resopnse
M2_pwm=0;  // test step resopnse
}

pwm_set(M1_dir,M1_pwm,M2_dir,M2_pwm);