Bouwverslag Gry Maritha

/* Gry Maritha Hatch speed regulation MULTIPROP, Fronthatch, Rearhatch, Sidehatch, Lift + lift failsave and Anchors versie 3.2.0 om 17:24
 * Failsave on Fronthatch vs Sidehatch / Sidehatch vs Lift / Rearhatch VS Heila Crane
 * 
 */


#include <VarSpeedServo.h>       

#define RCmax      2200         
#define RCmid      1500           
#define RCmin       800         
#define TimeOut  260000         

#define Closepos1           165  // Fronthatch 7 165
#define Openpos1              7  // Open positie
#define Savepos1            165  //
#define Speed1                1  // =57s

#define Closepos2            27  // RearHatch 27 165
#define Openpos2            165  // Open positie
#define Savepos2             27  //
#define Speed2                1  // =54s

#define Closepos3           140  // Sidehatch 28 135
#define Openpos3             30  //
#define Savepos3Lift         40  //
#define Savepos3FrontHatch  140  //
#define Speed3                2  // = 22s

#define Uppos4               37  // Lift
#define Midpos4              60  // Lift 37 60 135
#define Downpos4            135  //
#define Speed4                3  // 5 = 9s 3 = 14s

#define Uppos5               42  // AnchorLeft
#define Hangpos5             47  // Juist buiten de anker kluis hangend
#define Downpos5            140  //
#define Speeddown5          255  // vallen
#define Speedup5              1  // ophalen

#define Uppos6              153  // AnchorRight
#define Hangpos6            150  //
#define Downpos6             60  //
#define Speeddown6          255  //
#define Speedup6              1  //

#define RCport1               4  // FRONTHATCH 4MP naar D4
#define RCport2               5  // REARHATCH 5MP naar D3
#define RCport3               2  // SIDEHATCH
#define RCport4               3  //
#define RCport5               6  // ANCHOR LEFT
#define RCport6               7  // ANCHOR RIGHT

#define SNDoutput1           14  // FRONTHATCH (A0 = D14 ) = SND naar USM EXT1
#define SNDoutput2           15  // REARRHATCH (A1 = D15) = SND naar USM EXT2
#define SNDoutput3           16  // SIDEHATCH SND = USM EXT3
#define SNDoutput4           17  //

#define CraneInput           18  // (A3144 magneet = LOW )

VarSpeedServo Fronthatch;       
VarSpeedServo Rearhatch;       
VarSpeedServo Sidehatch;         
VarSpeedServo Lift;             
VarSpeedServo Anchorleft;       
VarSpeedServo Anchorright;       

int RCpulse1;                   
int RCpulse2;
int RCpulse3;
int RCpulse4;
int RCpulse5;
int RCpulse6;

int Cranepos;

int Pos1;   
int Pos2;
int Pos3;
int Pos4;
int Pos5;
int Pos6;

int Speed5;
int Speed6;

int Pos1Read;
int Pos2Read;
int Pos3Read;
int Pos4Read;
int Pos5Read;
int Pos6Read;



  void setup()
{

// for (int Smoothning = 0; Smoothning < Smoothfactor; Smoothning++){Smoothfactor = 0;}  // proberen de map te stabilizeren

  Pos1 = Closepos1;      // Start waarde voor servo positie FrontHatch (dicht)
  Pos2 = Closepos2;      // Start waarde voor servo positie Rearhatch (dicht)
  Pos3 = Closepos3;      // Start waarde voor servo positie Sidehatch (dicht)
  Pos4 = Downpos4;       // Start waarde voor servo positie Lift  (onder)
  Pos5 = Hangpos5;       // Start waarde voor servo positie AnchorLeft  (hangen)
  Pos6 = Uppos6;         // Start waarde voor servo positie AnchorRight  (op)

  Pos1Read = Openpos1;   // FrontHatch  Hier geven we de initiele waarden van de positie read aan,
  Pos2Read = Openpos2;   // RearHatch
  Pos3Read = Openpos3;   // SideHatch

  Speed5 = Speedup5;     // De ankersnelheid
  Speed6 = Speeddown6;   //

  Fronthatch.attach   (8);                    //plaats de servo op pin 8
  Fronthatch.write(Pos1,Speed1,false);        // deze regel zorgt ervoor dat tijdens de start de servo naar de gewenste positie gaat of blijft op deze postitie en niet centered

  Rearhatch.attach    (9);   
  Rearhatch.write(Pos2,Speed2,false);

  Sidehatch.attach   (10);   
  Sidehatch.write(Pos3,Speed3,false);

  Lift.attach        (11);
  Lift.write(Pos4,Speed4,false);

  Anchorleft.attach  (12);
  Anchorleft.write(Pos5,Speed5,false);

  Anchorright.attach (13);
  Anchorright.write(Pos6,Speed6,false);

  pinMode(SNDoutput1, OUTPUT);
  pinMode(SNDoutput2, OUTPUT);
  pinMode(SNDoutput3, OUTPUT);
  pinMode(SNDoutput4, OUTPUT);           // nog niet gebruikt

  pinMode(CraneInput, INPUT);            // De A3144 van de kraan is input
 
  digitalWrite(SNDoutput1, HIGH);        // omdat de USM de neutraal of 0 schakeld zetten we de uitgang hoog dit is 5v
  digitalWrite(SNDoutput2, HIGH);
  digitalWrite(SNDoutput3, HIGH);
  digitalWrite(SNDoutput4, HIGH);

// Serial.begin(9600);

}

  void RCread(){ // hier worden de RC inputs ingelezen
  RCpulse1 = pulseIn(RCport1, HIGH, TimeOut); 
  RCpulse2 = pulseIn(RCport2, HIGH, TimeOut); 
  RCpulse3 = pulseIn(RCport3, HIGH, TimeOut);
  RCpulse4 = pulseIn(RCport4, HIGH, TimeOut);
  RCpulse5 = pulseIn(RCport5, HIGH, TimeOut);
  RCpulse6 = pulseIn(RCport6, HIGH, TimeOut);

  Cranepos = digitalRead(CraneInput);         
 
  }

   void loop()
  {
    RCread(); // Ga naar RCread, om de RC inputs in te lezen.
  
    if(RCpulse1 > RCmin && RCpulse1 < RCmax){
      
      if(Pos3Read >= Savepos3FrontHatch){                                                     
        if(RCpulse1 > RCmid + 250){Pos1 = Openpos1; digitalWrite(SNDoutput1, LOW);}                 
        else if(RCpulse1 < RCmid - 250){Pos1 = Closepos1; digitalWrite(SNDoutput1, LOW);}
        else {Pos1 = map(RCpulse1, (RCmid +249 ), (RCmid - 249), Openpos1, Closepos1); digitalWrite(SNDoutput1, HIGH);}}                                               
      else {Pos1 = Closepos1;} 

      if (Cranepos == HIGH){                           
        if(RCpulse2 > RCmid + 200){Pos2 = Openpos2; digitalWrite(SNDoutput2, LOW);}                   
        else if(RCpulse2 < RCmid - 200){Pos2 = Closepos2;digitalWrite(SNDoutput2, LOW);}
        else {digitalWrite(SNDoutput2, HIGH);}}
      else {Pos2 = Closepos2;}

      if(Pos1Read == Closepos1){                                                                     
        if(RCpulse3 > RCmid + 200){Pos3 = Closepos3;digitalWrite(SNDoutput3, LOW);}                 
        else if(RCpulse3 < RCmid - 200){Pos3 = Openpos3;digitalWrite(SNDoutput3, LOW);}}
      else {Pos3 = Closepos3;}

            if(Pos3Read <= Savepos3Lift){                                                               
        if(RCpulse4 < RCmid - 100){Pos4 = Uppos4;}
        else if(RCpulse4 > RCmid + 100){Pos4 = Downpos4;}
        else {Pos4 = Midpos4;}}
      if (Pos3Read > Savepos3Lift){Pos4 = Downpos4;}

      if(RCpulse5 > RCmid + 100){Pos5 = Uppos5; Speed5 = Speedup5;}                                   
      else if(RCpulse5 < RCmid + 100 && RCpulse5 > RCmid - 50){Pos5 = Hangpos5; Speed5 = Speedup5;} 
      else if(RCpulse5 < RCmid - 250){Pos5 = Downpos5; Speed5 = Speeddown5;}
      else{Pos5 = map(RCpulse5, (RCmid -50 ), (RCmid - 250), Hangpos5, Downpos5); Speed5 = Speedup5;} 
 
      if(RCpulse6 > RCmid + 100){Pos6 = Uppos6; Speed6 = Speedup6;} 
      else if(RCpulse6 < RCmid + 100 && RCpulse6 > RCmid - 50){Pos6 = Hangpos6; Speed6 = Speedup6;}   
      else if(RCpulse6 < RCmid - 250){Pos6 = Downpos6; Speed6 = Speeddown6;}
      else{Pos6 = map(RCpulse6, (RCmid -50 ), (RCmid - 250), Hangpos6, Downpos6); Speed6 = Speedup6;}
  

//Serial.print("RCpulse5 = "); // hiermee kan je de pulses meten en tonen als het bord is aangesloten op de seriele monitor.
//Serial.println(RCpulse5);
//delay(1000);

 
    }

    Fronthatch.write(Pos1,Speed1,false);             // laat de servo naar de variabele 'pos1' gaan
    Rearhatch.write(Pos2,Speed2,false);             
    Sidehatch.write(Pos3,Speed3,false);
    Lift.write(Pos4,Speed4,false);     
    Anchorleft.write(Pos5,Speed5,false);
    Anchorright.write(Pos6,Speed6,false);             

    Pos1Read = Fronthatch.read();                     // lees de positie van de servo
    Pos2Read = Rearhatch.read();                     
    Pos3Read = Sidehatch.read();                     
 

  }

  Rearhatch.attach    (9);  
  Rearhatch.write(Pos2);

VarSpeedServo::VarSpeedServo()
{
if( ServoCount < MAX_SERVOS) {
this->servoIndex = ServoCount++; // assign a servo index to this instance
  servos[servoIndex].ticks = usToTicks(1100);   // store new start servo1
  servos[servoIndex].ticks = usToTicks(1200);   // store new start servo2
  servos[servoIndex].ticks = usToTicks(1300);   // store new start servo3
  servos[servoIndex].ticks = usToTicks(1400);   // store new start servo4
  servos[servoIndex].ticks = usToTicks(1500);   // store new start servo5
  servos[servoIndex].ticks = usToTicks(1600);   // store new start servo6
this->curSeqPosition = 0;
this->curSequence = initSeq;
}
else
this->servoIndex = INVALID_SERVO ; // too many servos
}

  Rearhatch.attach    (9);   
  Rearhatch.write(Pos2);

  void setup()
{


  Pos1 = Closepos1;      // Start waarde voor servo positie FrontHatch (dicht)
  Pos2 = Closepos2;      // Start waarde voor servo positie Rearhatch (dicht)
  Pos3 = Closepos3;      // Start waarde voor servo positie Sidehatch (dicht)
  Pos4 = Downpos4;       // Start waarde voor servo positie Lift  (onder)
  Pos5 = Hangpos5;       // Start waarde voor servo positie AnchorLeft  (hangen)
  Pos6 = Hangpos6;       // Start waarde voor servo positie AnchorRight  (op)
  Pos7 = Midpos7; 
  Pos8 = Midpos8;
 
  Pos1Read = Openpos1;   // FrontHatch  Hier geven we de initiele waarden van de positie read aan, deze wordt naderhand
  Pos2Read = Openpos2;   // RearHatch
  Pos3Read = Openpos3;   // SideHatch
 

  Speed5 = Speedup5;     // De ankersnelheid
  Speed6 = Speeddown6;   // Idem

  Fronthatch.attach   (10);    //plaats de servo op pin 10
  Fronthatch.write(Pos1,Speed1,false); // deze regel zorgt ervoor dat tijdens de start de servo naar de gewenste positie gaat of blijft op deze postitie en niet centered

  Rearhatch.attach    (11);    //plaats de servo op pin 11...
  Rearhatch.write(Pos2,Speed2,false);

  Sidehatch.attach    (12);   
  Sidehatch.write(Pos3,Speed3,false);

  Lift.attach         (13);
  Lift.write(Pos4,Speed4,false);

  Anchorleft.attach   (14);
  Anchorleft.write(Pos5,Speed5,false);

  Anchorright.attach  (15);               // GEETECH KAN ANALOG 14 15 16 17
  Anchorright.write(Pos6,Speed6,false);

  HeilaRotate.attach  (16);               //
  HeilaRotate.write(Pos7,Speed7,false);

  HeilaBoom.attach    (17);                //
  HeilaBoom.write(Pos8,Speed8,false);
 
  pinMode(SNDoutput1, OUTPUT); //hier worden de SND uitgangen gedefinieerd, deze gaan naar de USM ext ingangen dit zijn er maximaal 4
  pinMode(SNDoutput2, OUTPUT);

  pinMode(CraneInput, INPUT);    // De hallsensor van de kraan is input poort 20 op de Arduino
 
  digitalWrite(SNDoutput1, HIGH); // omdat de USM de neutraal of 0 schakeld zetten we de uitgang hoog dit is 5v
  digitalWrite(SNDoutput2, HIGH);


//Serial.begin(9600); // dit maak te connectie met de

}

  void setup()
{


  Pos1 = Closepos1;      // Start waarde voor servo positie FrontHatch (dicht)
  Pos2 = Closepos2;      // Start waarde voor servo positie Rearhatch (dicht)
  Pos3 = Closepos3;      // Start waarde voor servo positie Sidehatch (dicht)
  Pos4 = Downpos4;       // Start waarde voor servo positie Lift  (onder)
  Pos5 = Hangpos5;       // Start waarde voor servo positie AnchorLeft  (hangen)
  Pos6 = Hangpos6;       // Start waarde voor servo positie AnchorRight  (op)
  Pos7 = Midpos7; 
  Pos8 = Midpos8;
 
  Pos1Read = Openpos1;   // FrontHatch  Hier geven we de initiele waarden van de positie read aan, deze wordt naderhand
  Pos2Read = Openpos2;   // RearHatch
  Pos3Read = Openpos3;   // SideHatch
 

  Speed5 = Speedup5;     // De ankersnelheid
  Speed6 = Speeddown6;   // Idem

  Fronthatch.attach   (10);    //plaats de servo op pin 10
  Fronthatch.write(Pos1); // deze regel zorgt ervoor dat tijdens de start de servo naar de gewenste positie gaat of blijft op deze postitie en niet centered

  Rearhatch.attach    (11);    //plaats de servo op pin 11...
  Rearhatch.write(Pos2);

  Sidehatch.attach    (12);   
  Sidehatch.write(Pos3);

  Lift.attach         (13);
  Lift.write(Pos4);

  Anchorleft.attach   (14);
  Anchorleft.write(Pos5);

  Anchorright.attach  (15);               // GEETECH KAN ANALOG 14 15 16 17
  Anchorright.write(Pos6);

  HeilaRotate.attach  (16);               //
  HeilaRotate.write(Pos7);

  HeilaBoom.attach    (17);                //
  HeilaBoom.write(Pos8);
 
  pinMode(SNDoutput1, OUTPUT); //hier worden de SND uitgangen gedefinieerd, deze gaan naar de USM ext ingangen dit zijn er maximaal 4
  pinMode(SNDoutput2, OUTPUT);

  pinMode(CraneInput, INPUT);    // De hallsensor van de kraan is input poort 20 op de Arduino
 
  digitalWrite(SNDoutput1, HIGH); // omdat de USM de neutraal of 0 schakeld zetten we de uitgang hoog dit is 5v
  digitalWrite(SNDoutput2, HIGH);


//Serial.begin(9600); // dit maak te connectie met de

}