Sure, this is the code i am using.
#define A 46 // the pin connected to the wire A of the coil A (or to the H-bridge pin controlling the same wire)
#define A_bar 48 // the pin connected to the wire A- of the coil A (or to the H-bridge pin controlling the same wire)
#define B 50 // the pin connected to the wire B of the coil A (or to the H-bridge pin controlling the same wire)
#define B_bar 52 // the pin connected to the wire B- of the coil A (or to the H-bridge pin controlling the same wire)
#define in1 44 // first control pin
#define in2 45 // second control pin
#define motorpin 43 // Pod door control pin
int m,c, in1a,in2a,motorpina =0; // variable storing the mode
int stepsperrevolution = 200;
#include <Servo.h>
Servo myservo;
int lowspeed =10000;
int midspeed =7000;
int highspeed= 5000;
int current_pos=50;
int pos = 0;
void wait(){ // stop rotation of the servo
digitalWrite(A, HIGH);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
}
void slow(){ //slow motor rotation
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (lowspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (lowspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (lowspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (lowspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (lowspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (lowspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (lowspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (lowspeed);
}
void medium(){ //medium motor rotation
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (midspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (midspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (midspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (midspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (midspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (midspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (midspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (midspeed);
}
void high(){
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (highspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (highspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (highspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, HIGH);
digitalWrite(B_bar, LOW);
delayMicroseconds (highspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, LOW);
delayMicroseconds (highspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, HIGH);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (highspeed);
digitalWrite(A, LOW);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (highspeed);
digitalWrite(A, HIGH);
digitalWrite(A_bar, LOW);
digitalWrite(B, LOW);
digitalWrite(B_bar, HIGH);
delayMicroseconds (highspeed);
}
void dooropen(){
if (current_pos != 150){
for (pos = 50; pos <= 150; pos += 1) { // goes from 0 degrees to 180 degrees
myservo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
}
current_pos=150;
}
void doorclose(){
if (current_pos != 50){
for (pos = 150; pos >= 50; pos -= 1) { // goes from 180 degrees to 0 degrees
myservo.write(pos); // tell servo to go to position in variable ‘pos’
delay(15); // waits 15ms for the servo to reach the position
}
}
current_pos=50;
}
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
pinMode(A, OUTPUT); // output pins to control motors
pinMode(A_bar, OUTPUT);
pinMode(B, OUTPUT);
pinMode(B_bar, OUTPUT);
pinMode(in1, INPUT); // input from the DAQ
pinMode(in2, INPUT); // input from the DAQ
pinMode(motorpin, INPUT); // motor control pin
in1a= digitalRead(in1);
in2a=digitalRead(in2);
motorpina = digitalRead(motorpin);
myservo.attach(9);
myservo.write(current_pos);
//Serial.print(“Door is open”);
}
void loop() {
// put your main code here, to run repeatedly:
if (motorpina == 1){
dooropen();
Serial.print(“Door is open”);
}
if (motorpina ==0){
doorclose();
Serial.print(“Door is Closed”);
}
if (in1a ==0 && in2a == 0){ // pause motors
m=1;
wait() ;
Serial.println(“Motors paused”);
}
if (in1a ==1 && in2a == 1){ //slow speed
slow();
Serial.println(“Slow Speed”);
}
if (in1a ==1 && in2a == 0){ //medium speed
medium();
Serial.println(“Medium Speed”);
}
if (in1a ==0 && in2a == 1){ //high speed
high();
Serial.println(“High Speed”);
}
}