HD4-Motor

// Arduino Type Leanardo
// Arduino Revision 1.8.7
// Action of HD-Motors : Pulling only

// PinNumbers Leonardo :
int OptSWD=1;           // Optical Switch
int OptSWC=2;           // Optical Switch
int HD4=3;              // HardDiscmotor PWM Output
int OptSWB=4;           // Optical Switch
int OptSWA=5;           // Optical Switch
int HD3=6;              // HardDiscmotor PWM Output
int SW3=9;              // PushbuttonSwitch (RunCounterClockWise)
int HD2=10;             // HardDiscmotor PWM Output
int HD1=11;             // HardDiscmotor PWM Output
int SW4=12;             // PushbuttonSwitch (future)
int SW5=13;             // PushbuttonSwitch (RunClockWise)
int PotSpeed=A0;        // PotentioMeter for SpeedAdjustment

// Variables :
int TimeOn;             // Duration of time Outputs ON
int Number;
int StateSW3;
int StateSW4;
int StateSW5;
int StateOptSWA;
int StateOptSWB;
int StateOptSWC;
int StateOptSWD;
int StatePotSpeed;
int Speed;           
boolean StartedCW;
boolean StartedCCW;

void setup() 
{   Serial.begin(9600);   
    pinMode (HD1, OUTPUT);
    pinMode (HD2, OUTPUT);
    pinMode (HD3, OUTPUT);
    pinMode (HD4, OUTPUT);  
    pinMode (SW3, INPUT);  digitalWrite(SW3, HIGH);       // Internal Pull up
    pinMode (SW4, INPUT);  digitalWrite(SW4, HIGH);       // Internal Pull up  
    pinMode (SW5, INPUT);  digitalWrite(SW5, HIGH);       // Internal Pull up
    pinMode (PotSpeed, INPUT);                            //  PotSpeed           
    pinMode (OptSWA, INPUT); digitalWrite(OptSWA, LOW);   // Internal Pull down 
    pinMode (OptSWB, INPUT); digitalWrite(OptSWB, LOW);   // Internal Pull down 
    pinMode (OptSWC, INPUT); digitalWrite(OptSWC, LOW);   // Internal Pull down  
    pinMode (OptSWD, INPUT); digitalWrite(OptSWD, LOW);   // Internal Pull down         
    TimeOn=0; StartedCW=0; StartedCCW=0;}
    
  

void loop() 
{   StateSW3=digitalRead(SW3); if(StateSW3==0) {RunCounterClockWise();} else {StopMotors(); StartedCCW=0;}  // 
    StateSW5=digitalRead(SW5); if(StateSW5==0) {RunClockWise();} else {StopMotors(); StartedCW=0;}          //     
//    DisplayMonitor();      // For Testing only
}


void RunClockWise()   // Oke Met PWM en Regelbare Speed         // Test
{   if (StartedCW==0){StartClockWise();}
    ReadOpticalSwitch(); 
    TimeOn=40;   // was 40    //250 Schokt // 100 Lopen Redelijk // 50-80 Loopt Redelijk  // 10 LooptSlecht // Beste is 50
    if (StateOptSWA==1) {analogWrite(HD1,0); analogWrite(HD2,0); analogWrite(HD3, Speed); analogWrite(HD4, Speed); delay(TimeOn);} //
    if (StateOptSWD==1) {analogWrite(HD1,0); analogWrite(HD2, Speed); analogWrite(HD3, Speed); analogWrite(HD4,0); delay(TimeOn);} //  
    if (StateOptSWC==1) {analogWrite(HD1, Speed); analogWrite(HD2, Speed); analogWrite(HD3,0); analogWrite(HD4,0); delay(TimeOn);}   
    if (StateOptSWB==1) {analogWrite(HD1, Speed); analogWrite(HD2,0); analogWrite(HD3,0); analogWrite(HD4, Speed); delay(TimeOn);} 
}


void RunCounterClockWise()   // Met PWM en Regelbare Speed         // 
{   if (StartedCCW==0){StartCounterClockWise();}
   ReadOpticalSwitch(); 
   TimeOn=40;   // was 40
   if (StateOptSWA==1) {analogWrite(HD1,0); analogWrite(HD2,Speed); analogWrite(HD3, Speed); analogWrite(HD4, 0); delay(TimeOn);} //
   if (StateOptSWB==1) {analogWrite(HD1,0); analogWrite(HD2, 0); analogWrite(HD3, Speed); analogWrite(HD4,Speed); delay(TimeOn);} 
   if (StateOptSWC==1) {analogWrite(HD1, Speed); analogWrite(HD2, 0); analogWrite(HD3,0); analogWrite(HD4,Speed); delay(TimeOn);}       
   if (StateOptSWD==1) {analogWrite(HD1, Speed); analogWrite(HD2,Speed); analogWrite(HD3,0); analogWrite(HD4, 0); delay(TimeOn);} //  
}

void StartClockWise()   // 
{   TimeOn=100;         // was 100
    analogWrite(HD4, 255);  delay(1000); analogWrite(HD4, 0);     // To StartPosition !!    
    for (Number=0; Number<4; Number++)  
         {analogWrite(HD3, 255);  delay(TimeOn); analogWrite(HD3, 0);   //
          analogWrite(HD2, 255);  delay(TimeOn); analogWrite(HD2, 0);   //  
          analogWrite(HD1, 255);  delay(TimeOn); analogWrite(HD1, 0);   
          analogWrite(HD4, 255);  delay(TimeOn); analogWrite(HD4, 0);}
    TimeOn=TimeOn-50; StartedCW=1;}

void StartCounterClockWise()   // 
{   TimeOn=100;      // was 100
    analogWrite(HD4, 255);  delay(1000); analogWrite(HD4, 0);     //  To StartPosition !!      
    for (Number=0; Number<4; Number++)
         {analogWrite(HD1, 255);  delay(TimeOn); analogWrite(HD1, 0);   //
          analogWrite(HD2, 255);  delay(TimeOn); analogWrite(HD2, 0);   //  
          analogWrite(HD3, 255);  delay(TimeOn); analogWrite(HD3, 0);   
          analogWrite(HD4, 255);  delay(TimeOn); analogWrite(HD4, 0);}
    TimeOn=TimeOn-50; StartedCCW=1;}

void DisplayMonitor()
{   StateSW3=digitalRead(SW3); Serial.print("SW3 = " );   Serial.print(StateSW3); 
    StateSW4=digitalRead(SW4); Serial.print("   SW4 = " );   Serial.print(StateSW4); 
    StateSW5=digitalRead(SW5); Serial.print("   SW5 = " );   Serial.print(StateSW5); 
    StateOptSWA=digitalRead (OptSWA); Serial.print("       OptSWA = " );   Serial.print(StateOptSWA);      
    StateOptSWB=digitalRead (OptSWB); Serial.print("\t OptSWB = "); Serial.print(StateOptSWB); 
    StateOptSWC=digitalRead (OptSWC); Serial.print("\t OptSWC = "); Serial.print(StateOptSWC); 
    StateOptSWD=digitalRead (OptSWD); Serial.print("\t OptSWD = "); Serial.print(StateOptSWD); 
    Speed=analogRead(PotSpeed)/4; Serial.print("\t Speed = "); Serial.print(Speed);        
    Serial.println();}

void ReadOpticalSwitch()    // 
{   StateOptSWA=digitalRead (OptSWA);
    StateOptSWB=digitalRead (OptSWB);
    StateOptSWC=digitalRead (OptSWC);
    StateOptSWD=digitalRead (OptSWD);
    Speed=analogRead(PotSpeed)/4;} 

void StopMotors()
{ analogWrite(HD1, 0); analogWrite(HD2, 0);
  analogWrite(HD3, 0); analogWrite(HD4, 0);}