Controlling Servo, Electromagnet & two DC motor's position using an encoder,

Hello ppl,

I was wondering if anyone could help me out a little bit, basically am new to the game of coding and am currently working on gantry crane for my college project, I need to control two DC motors a servo and electromagnet from one push button then return to the rest position. the crane will use the X-Y cordinate system similar to CNC. so the problem is have this piece of code that i found earlier for inspiration seemed like it could help I would like to incorporate the DC motors, Electronmagnet and servo to this code so they can work in a sequence.

/ Interrupt information
// 0 on pin 2
// 1 on pin 3

#define encoderI 2
#define encoderQ 4 // Only use one interrupt in this example

volatile int count;
void setup()
{
Serial.begin(115200);
count=0;
pinMode(encoderI, INPUT);
pinMode(encoderQ, INPUT); attachInterrupt(0, handleEncoder, CHANGE);

}

void loop()
{
Serial.println(count);
delay(10);
}

void handleEncoder()
{
if(digitalRead(encoderI) == digitalRead(encoderQ))
{ count++;
}
else
{ count–;
}

}

So played around with the some CNC code and Incorporated the encoder code but i am trying to add a servo to this but every time i try is messes up the code. i would like the servo to be turned on when the second motor stops at its position. Can anyone help me out please?

//#include <avr/io.h>
//#include <util/delay.h>
#include <Servo.h>

//Change values below to adapt your motor

#define STEP_MARGIN 1L //10 - 1000 (2)
#define MIN_DUTYCYCLE 75 //0 - 255 (125)
#define MAX_DUTYCYCLE 255 //0 - 255 (255)

#define M1_SENSOR_1 12 //Sensor input 1
#define M1_SENSOR_2 13 //Sensor input 2
#define M1_DIRECTION 4 //Direction output motor
#define M1_PWM 5 //PWM output motor

#define M2_SENSOR_1 10 //Sensor input 1
#define M2_SENSOR_2 11 //Sensor input 2
#define M2_DIRECTION 2 //Direction output motor
#define M2_PWM 3 //PWM output motor

#define RELAY A1 //Relay used to turn on milling machine

int M1Sensor02 = 0;
int M1Sensor01 = 0;
int M1DutyCycle = 100; // 10 - 255
unsigned long M1SetPoint = 0;
unsigned long M1IsPoint = 0;
byte M1Step = 0;
byte M1StepDone = 0;

int M2Sensor02 = 0;
int M2Sensor01 = 0;
int M2DutyCycle = 100; // 10 - 255
unsigned long M2SetPoint = 0;
unsigned long M2IsPoint = 0;
byte M2Step = 0;
byte M2StepDone = 0;

byte readByte = 0;

unsigned long stepTime = 1;

unsigned long millisStart = 0;

void establishContact() {
while (Serial.available() <= 0) {
Serial.print(‘X’); // send a capital X
delay(300);
}
}

void setup(){

pinMode(RELAY, OUTPUT);
digitalWrite(RELAY, 0);

pinMode(M1_DIRECTION, OUTPUT);
pinMode(M1_PWM, OUTPUT);
analogWrite(M1_PWM, 0);
digitalWrite(M1_DIRECTION, 0);

pinMode(M1_SENSOR_1, INPUT);
pinMode(M1_SENSOR_2, INPUT);

pinMode(M2_DIRECTION, OUTPUT);
pinMode(M2_PWM, OUTPUT);
analogWrite(M2_PWM, 0);
digitalWrite(M2_DIRECTION, 0);

pinMode(M2_SENSOR_1, INPUT);
pinMode(M2_SENSOR_2, INPUT);

stepTime = 1;
readByte = 0;

M1Step = 0;
M2Step = 0;

M1StepDone = 1;
M2StepDone = 1;

M1SetPoint = 300000;
M1IsPoint = 300000;

M2SetPoint = 300000;
M2IsPoint = 300000;

// start serial port at 115200 bps:
Serial.begin(115200);
establishContact(); // send a byte to establish contact until receiver responds
}

void loop(){

millisStart = millis();
while(millis() - millisStart < stepTime){
do{
M1Sensor01 = digitalRead(M1_SENSOR_1);
M1Sensor02 = digitalRead(M1_SENSOR_2);

if(M1Sensor02 == 0 && M1Sensor01 == 0){
if(M1Step == 1){
M1IsPoint–;
}
if(M1Step == 3){
M1IsPoint++;
}
M1Step = 0;
}

if(M1Sensor02 == 1 && M1Sensor01 == 0){
if(M1Step == 0){
M1IsPoint++;
}
if(M1Step == 2){
M1IsPoint–;
}
M1Step = 1;
}

if(M1Sensor02 == 1 && M1Sensor01 == 1){
if(M1Step == 3){
M1IsPoint–;
}
if(M1Step == 1){
M1IsPoint++;
}
M1Step = 2;
}

if(M1Sensor02 == 0 && M1Sensor01 == 1){
if(M1Step == 2){
M1IsPoint++;
}
if(M1Step == 0){
M1IsPoint–;
}
M1Step = 3;
}

if(abs(M1SetPoint - M1IsPoint) < STEP_MARGIN){
analogWrite(M1_PWM, 0);
digitalWrite(M1_DIRECTION, 0);
M1StepDone = 1;
M1DutyCycle = MIN_DUTYCYCLE;
}
else{
if(M1IsPoint < M1SetPoint){
digitalWrite(M1_DIRECTION, 1);
analogWrite(M1_PWM, 255 - M1DutyCycle);
}
if(M1IsPoint > M1SetPoint){
digitalWrite(M1_DIRECTION, 0);
analogWrite(M1_PWM, M1DutyCycle);
}
}

M2Sensor01 = digitalRead(M2_SENSOR_1);
M2Sensor02 = digitalRead(M2_SENSOR_2);

if(M2Sensor02 == 0 && M2Sensor01 == 0){
if(M2Step == 1){
M2IsPoint–;
}
if(M2Step == 3){
M2IsPoint++;
}
M2Step = 0;
}

if(M2Sensor02 == 1 && M2Sensor01 == 0){
if(M2Step == 0){
M2IsPoint++;
}
if(M2Step == 2){
M2IsPoint–;
}
M2Step = 1;
}

if(M2Sensor02 == 1 && M2Sensor01 == 1){
if(M2Step == 3){
M2IsPoint–;
}
if(M2Step == 1){
M2IsPoint++;
}
M2Step = 2;
}

if(M2Sensor02 == 0 && M2Sensor01 == 1){
if(M2Step == 2){
M2IsPoint++;
}
if(M2Step == 0){
M2IsPoint–;
}
M2Step = 3;
}

if(abs(M2SetPoint - M2IsPoint) < STEP_MARGIN){
analogWrite(M2_PWM, 0);
digitalWrite(M2_DIRECTION, 0);
M2StepDone = 1;
M2DutyCycle = MIN_DUTYCYCLE;
}
else{
if(M2IsPoint < M2SetPoint){
digitalWrite(M2_DIRECTION, 1);
analogWrite(M2_PWM, 255 - M2DutyCycle);
}
if(M2IsPoint > M2SetPoint){
digitalWrite(M2_DIRECTION, 0);
analogWrite(M2_PWM, M2DutyCycle);
}
}
}while(M1StepDone == 0 || M2StepDone == 0);
if(readByte == 0){
millisStart = 0L;
}
}//while(millis() - millisStart < stepTime){

readByte = 0;
if (Serial.available() > 0){//if we get a valid byte, read from serial:
//get incoming byte:
readByte = Serial.read();
Serial.print(‘r’); // send a ‘r’ to initiate next data from computer
}//if (Serial.available() > 0){

if(readByte > 0){
if(readByte == 255){
digitalWrite(RELAY, 1);
readByte = 0;
}
if(readByte == 254){
digitalWrite(RELAY, 0);
readByte = 0;
}

if(readByte == 128){
if(stepTime < 1000){
stepTime+=10;
}
}
if(readByte == 64){
if(stepTime > 10){
stepTime-=10;
}
}
if(readByte == 32){
M1SetPoint++;
M1DutyCycle = MAX_DUTYCYCLE;
M1StepDone = 0;
}
if(readByte == 16){
M1SetPoint–;
M1DutyCycle = MAX_DUTYCYCLE;
M1StepDone = 0;
}
if(readByte == 8){
M2SetPoint++;
M2DutyCycle = MAX_DUTYCYCLE;
M2StepDone = 0;
}
if(readByte == 4){
M2SetPoint–;
M2DutyCycle = MAX_DUTYCYCLE;
M2StepDone = 0;
}
}//if(readByte > 0){

}

 Serial.print('X');   // send a capital X

Great comment.
Where are the rest of them?
Where are the code tags?

int M1DutyCycle = 100; // 10 - 255Not a great comment

Thanks for pointing that out, but i didn't really change anything. I need to control two DC motors, servo and an electromagnet from one push button then return to the resting position. The jib crane will use the X-Y coordinate system similar to CNC, so the problem am having is that this piece of code i altered earlier worked but I would like to incorporate an Electromagnet and servo to this code so they can work in a sequence. would you be able to help??

Thanks for pointing that out, but i didn't really change anything

Why not?
You could at least have used code tags.

no sorry, I meant i did add them in but it didn't really change anything.

Alex3900:
no sorry, I meant i did add them in but it didn't really change anything.

You added what in?