Here's nearly all the sketch. I've removed the bits that don't do anything yet.
// 009 reverse loop
// 10/09/2023 v010
// SERVO LIBRARY - loads servo control library
#include <Servo.h>
// ASSIGN PINS
int servoPinL = 38; Servo ServoL; // Yellow
int servoPinU = 30; Servo ServoU; // Yellow
int LOOPL_IR = 47; // White
int LOOPU_IR = 39; // White
int HILLTOP_IR = 13; // White
int HILLBOT_IR = 12; // yellow
int LOOPS_PWM = 3; // PWM output to loops is on pin 3 MOTOR B Grey ENABLE B
int LOOPS_CW = 22; // pin 22 controls direction of travel on loops Purple (input 4)
int LOOPS_ACW = 2; // pin 2 controls direction of travel on loops Blue (input 3)
int TWOWAY_PWM = 5; // PWM output to 2-way section is on pin 5 Motor A Green ENABLE A
int TWOWAY_UP = 8; // pin 8 controls polarity on two-way section. Yellow (Input 1)
int TWOWAY_DOWN = 46; // pin 46 controls polarity on two-way section. Orange (Input 2)
int FROGU = 23; // pin 23 switches upper frog to yellow (outer rail)
int FROGL = 31; // pin 31 switches lower frog to white (inner rail)
int SERVO_ISO = 48; // pin 48 relay L1 to isolate servos (yellow)
// ASSIGN NUMERIC VALUES
int TRAINSPEED = 90;
int UPSPEED = 120;
int DOWNSPEED = 55;
int SPEEDUP = (((TRAINSPEED-DOWNSPEED)/2)+DOWNSPEED);
int dec = 70;
int acc = 70;
void setup() {
Serial.begin(9600);
// INITIALISE PINS
ServoL.attach(servoPinL); // Sets up 'servoPinL' as output for LOWER POINT
ServoU.attach(servoPinU); // Sets up 'servoPinU' as output for UPPER POINT
pinMode(LOOPL_IR, INPUT);
pinMode(LOOPU_IR, INPUT);
pinMode(HILLTOP_IR, INPUT_PULLUP); //NOTE: I called this SENSOR2 in my example
pinMode(HILLBOT_IR, INPUT_PULLUP); //NOTE: I called this SENSOR1 in my example
pinMode(LOOPS_PWM, OUTPUT); // Output to loops PWM 'ENABLE B'
pinMode(LOOPS_CW, OUTPUT); // Output to loops polarity input 1
pinMode(LOOPS_ACW, OUTPUT); // Output to loops polarity input 2
pinMode(TWOWAY_PWM, OUTPUT); // Output to 2-way PWM 'ENABLE A'
pinMode(TWOWAY_UP, OUTPUT); // Output to 2-way polarity input 3
pinMode(TWOWAY_DOWN, OUTPUT); // Output to 2-way polarity input 4
pinMode(FROGL, OUTPUT); // Output to RELAY 1
pinMode(FROGU, OUTPUT); // Output to RELAY 8
pinMode(SERVO_ISO, OUTPUT); // output to relay L1 (yellow)
// DEFAULTS - the list below is part of setup - define defaults before programe code loop begins
analogWrite(LOOPS_PWM, 0); // loops PWM OFF
analogWrite(TWOWAY_PWM, 0); // 2-way PWM OFF
ENDSCW(); // sets loops polarity to CW
digitalWrite(TWOWAY_UP, LOW); digitalWrite(TWOWAY_DOWN, HIGH); // sets 2-way section to UP
digitalWrite(FROGL, HIGH);
digitalWrite(FROGU, LOW); // Sets lower frog to right white, Sets upper frog to left yellow
digitalWrite(SERVO_ISO, 1); //turns off power to servos
}
//____________________________________ MAIN LOOP ______________________________________
void loop() {
ENDSCW(); // Both loops set to CW
GOINGUP(); // Prep points etc to go UP
delay(500);
UP(); delay(500); // sets polarity & speed to go UP
Serial.println("HILLBOT UP");
HILLBOTup(); delay(500);
Serial.println("HILLTOP UP");
HILLTOPup();
LOOPU(); // waits for upper sensor to be tripped
GOINGDOWN(); // Prep points etc to go down
delay(500);
DOWN(); // sets polarity & speed to go down
Serial.println("HILLTOP DOWN");
HILLTOPdown(); delay(500);
Serial.println("HILLBOT DOWN");
HILLBOTdown();
LOOPL(); // waits for lower sensor to be tripped
}
// ____________________ SUBROUTINES _______________________
void STOP() {
analogWrite(LOOPS_PWM, 0); // loops PWM OFF
analogWrite(TWOWAY_PWM, 0); // 2-way PWM OFF
}
// Sets LOOPS polarity to CW
void ENDSCW(){
analogWrite(LOOPS_PWM, TRAINSPEED);
digitalWrite(LOOPS_CW, LOW); digitalWrite(LOOPS_ACW, HIGH); // sets loops polarity to CW
}
// This sub sets points to go UP
void GOINGUP(){
TWOWAYOFF(); // 2-way power off
digitalWrite(FROGL, HIGH); // sets frog
digitalWrite(FROGU, LOW); // relay ON sets frog LEFT **********
digitalWrite(SERVO_ISO, 0); //turns on power to servos
ServoU.write(115);delay(500); // upper point LEFT
ServoL.write(105);delay(500); // changes lower point
digitalWrite(SERVO_ISO, 1); //turns off power to servos
}
void UP(){
TWOWAYOFF(); // two-way power OFF
digitalWrite(TWOWAY_UP, LOW); digitalWrite(TWOWAY_DOWN, HIGH); // sets 2-way section to UP
analogWrite(TWOWAY_PWM, TRAINSPEED); // two-way power ON
}
void HILLBOTup(){
// Checks HILLBOT sensor (HILLBOT SENSOR is SENSOR1)
Serial.println(digitalRead(HILLBOT_IR));
while (digitalRead(HILLBOT_IR) == 1){delay(50);}
analogWrite(TWOWAY_PWM, UPSPEED); // two-way power ON
}
void HILLTOPup(){
// Checks HILLTOP sensor (HILLTOP is SENSOR2)
Serial.println(digitalRead(HILLTOP_IR));
while (digitalRead(HILLTOP_IR) == 1){delay(50);}
analogWrite(TWOWAY_PWM, TRAINSPEED); // two-way power ON
}
void LOOPU(){
while (digitalRead(LOOPU_IR) == 1){delay(20);} // waits until upper loop IR is tripped
}
// This sub sets points to go DOWN
void GOINGDOWN(){
TWOWAYOFF(); // 2-way power off
digitalWrite(SERVO_ISO, 0); //turns on power to servos
ServoU.write(125);delay(500); //sets point RIGHT
digitalWrite(FROGU, HIGH); // sets frog
digitalWrite(FROGL, LOW); // sets frog
ServoL.write(85);delay(500);
digitalWrite(SERVO_ISO, 1); //turns off power to servos
}
void DOWN(){
TWOWAYOFF();
digitalWrite(TWOWAY_UP, HIGH); digitalWrite(TWOWAY_DOWN, LOW); // sets 2-way section to DOWN
analogWrite(TWOWAY_PWM, TRAINSPEED);
delay(1000);
}
void HILLTOPdown(){
// checks HILLTOP sensor
Serial.println(digitalRead(HILLTOP_IR));
while (digitalRead(HILLTOP_IR) == 1){delay(50);}
analogWrite(TWOWAY_PWM, DOWNSPEED); // slows train on descent
}
void HILLBOTdown(){
Serial.println(digitalRead(HILLBOT_IR));
while (digitalRead(HILLBOT_IR) ==1){delay(10);}
analogWrite(TWOWAY_PWM, TRAINSPEED); // increases voltage after descent
}
void LOOPL(){
while (digitalRead(LOOPL_IR) == 1){delay(50);}
}
// _______________________________________________
void TWOWAYOFF() {
analogWrite(TWOWAY_PWM, 0); // 2-way PWM OFF
}
void LOOPSACW(){
analogWrite(LOOPS_PWM, TRAINSPEED);
digitalWrite(LOOPS_CW, HIGH); digitalWrite(LOOPS_ACW, LOW); // sets loops polarity to ACW
}
