#define DIRECTION_M1_1 5 #define DIRECTION_M1_2 6 #define PWM_M1 3 #define MOTOR_1 PWM_M1 #define DIRECTION_M2_1 7 #define DIRECTION_M2_2 8 #define PWM_M2 9 #define MOTOR_2 PWM_M2 #define PWM_IN_1 10 #define PWM_IN_2 11 #define BACKWARD false #define FORWARD true boolean dirM1; boolean dirM2; int pwm_value_1 = 0; int pwm_value_2 = 0; void setup() { pinMode(DIRECTION_M1_1, OUTPUT); pinMode(DIRECTION_M1_2, OUTPUT); pinMode(PWM_M1, OUTPUT); pinMode(DIRECTION_M2_1, OUTPUT); pinMode(DIRECTION_M2_2, OUTPUT); pinMode(PWM_M2, OUTPUT); stop(MOTOR_1); stop(MOTOR_2); pinMode(PWM_IN_1, INPUT); pinMode(PWM_IN_2, INPUT); } void loop() { pwm_value_1 = pulseIn(PWM_IN_1, HIGH); pwm_value_2 = pulseIn(PWM_IN_2, HIGH); pwm_value_1 = map(pwm_value_1, 1000, 2000, -255, 255); pwm_value_2 = map(pwm_value_2, 1000, 2000, -255, 255); if (pwm_value_1 < 10 && pwm_value_1 > -10) { stop(MOTOR_1); } else { setMotor(MOTOR_1, pwm_value_1); } if (pwm_value_2 < 10 && pwm_value_2 > -10) { stop(MOTOR_2); } else { setMotor(MOTOR_2, pwm_value_2); } delay(20); } void stop(int motor) { analogWrite(motor, 0); } void setMotor(int motor, int pwm) { boolean dir; if (pwm < 0) { dir = BACKWARD; pwm *= -1; } else { dir = FORWARD; } if (motor == MOTOR_1) { if (dirM1 != dir) { setDirection(MOTOR_1, dir); } setSpeed(MOTOR_1, pwm); } else { if (dirM2 != dir) { setDirection(MOTOR_2, dir); } setSpeed(MOTOR_2, pwm); } } void setDirection(int motor, boolean dir) { if (motor == MOTOR_1) { digitalWrite(DIRECTION_M1_1, dir); digitalWrite(DIRECTION_M1_2, !dir); dirM1 = dir; } else { digitalWrite(DIRECTION_M2_1, dir); digitalWrite(DIRECTION_M2_2, !dir); dirM2 = dir; } } void setSpeed(int motor, int sp) { if (motor == MOTOR_1) { analogWrite(PWM_M1, sp); } else { analogWrite(PWM_M2, sp); } }