4 wiel aandrijving robot vraag

[Pronkie Profiel Privébericht]

Beste Forumleden,

Ik ben sinds kort een project begonnen alleen nu zit ik met een probleem. Ik heb een robot die word aangedreven door 4 wielen en hij word bestuurd via de pc en nou gaat het naar voren en naar achter toe rijden wel gewoon goed maar zodra ik wil gaan sturen doet die dat niet en heb al een aantal dingen geprobeerd maar kom er niet uit. Kunnen jullie mij helpen ?

Hieronder de code

Code: Alles selecteren

int normalSpeed = 8000; // General speed
int spinSpeed = 2000; // Spins Speed


void setup(){
  Serial.begin(9600);
}


void loop(){
  if (Serial.available() > 0) {
  char motors = Serial.read();
   switch(motors) {
    case 'a':
        analogWrite(5,0);
        analogWrite(6,normalSpeed);
        analogWrite(9,0);
        analogWrite(10,normalSpeed);
     break;
    case 'd':
        analogWrite(5,normalSpeed);
        analogWrite(6,0);
        analogWrite(9,normalSpeed);
        analogWrite(10,0);
     break;
    case 'w':
        analogWrite(5,0);
        analogWrite(6,spinSpeed);
        analogWrite(9,spinSpeed);
        analogWrite(10,0);
        analogWrite(2,0);
        analogWrite(3,spinSpeed);
        analogWrite(12,spinSpeed);
        analogWrite(13,0);
     break;
    case 's':
        analogWrite(5,spinSpeed);
        analogWrite(6,0);
        analogWrite(9,0);
        analogWrite(10,spinSpeed);
        analogWrite(2,spinSpeed);
        analogWrite(3,0);
        analogWrite(12,0);
        analogWrite(13,spinSpeed);
     break;
   case 'q':
        analogWrite(5,0);
        analogWrite(6,0);
        analogWrite(9,0);
        analogWrite(10,0);
        analogWrite(2,0);
        analogWrite(3,0);
        analogWrite(12,0);
        analogWrite(13,0);
      break;
   } 
  }
}

Hieronder het schema




mvg, Anthony

[astrofrostbyte Profiel Privébericht]

Hey Pronkie, check de analogWrite functie.

int normalSpeed = 8000;
analogWrite(6,normalSpeed);

analogWrite(pin, value)
value: the duty cycle: between 0 (always off) and 255 (always on).


Voor de leesbaarheid van de code kan je de analog pin nummers ook nog leesbare namen geven bv M1LV, M3RA of zoiets.

[Pronkie Profiel Privébericht]

Beste Astrofrostbye,

Ben eigenlijk nog een leek op arduino gebied en heb na een paar uur het naar voor en het naar achter rijden onder de knie gekregen om een code te maken. Alleen zou u mij kunnen helpen om het naar links te sturen en het naar rechts sturen te kunnen herschrijven ? Als het voertuig naar links moet dan moeten moter 1 en 4 draaien moter 1 moet dan naar achter draaien en moter 4 naar voren en dan als het voertuig naar rechts moet dan moet moter 3 naar achter en moter 2 naar voren. Hoe kan ik dat in de bestaande code verwerken en zou u me kunnen helpen ?

mvg anthony

[astrofrostbyte Profiel Privébericht]

Even voor mezelf een oefening gedaan, Wheel library aangemaakt. een wheel kan forward, backward en idle, snelheid kan je meegeven :

Code: Alles selecteren

class Wheel
{ public:
    Wheel(int pina,int pinb);
    void forward(byte spin);
    void backward(byte spin);
    void idle();
  private:
    int  _pina;
    int  _pinb;
    byte _spin;
};

Wheel::Wheel(int pina, int pinb)
{ _pina = pina;
  _pinb = pinb;
  _spin = 0;
}
void Wheel::forward(byte spin)
{ _spin = spin;
  analogWrite(_pina, 0    );             
  analogWrite(_pinb, _spin);
}
void Wheel::backward(byte spin)
{ _spin = spin;
  analogWrite(_pina, _spin);             
  analogWrite(_pinb, 0    );
}
void Wheel::idle()
{ _spin = 0;
  analogWrite(_pina, 0 );             
  analogWrite(_pinb, 0 );
}


De Wheel library gebruikt in jou code :

Code: Alles selecteren

//-----------------------------------------= Wheel Library =----
class Wheel
{ public:
    Wheel(int pina,int pinb);
    void forward(byte spin);
    void backward(byte spin);
    void idle();
  private:
    int  _pina;
    int  _pinb;
    byte _spin;
};

Wheel::Wheel(int pina, int pinb)
{ _pina = pina;
  _pinb = pinb;
  _spin = 0;
}
void Wheel::forward(byte spin)
{ _spin = spin;
  analogWrite(_pina, 0    );             
  analogWrite(_pinb, _spin);
}
void Wheel::backward(byte spin)
{ _spin = spin;
  analogWrite(_pina, _spin);             
  analogWrite(_pinb, 0    );
}
void Wheel::idle()
{ _spin = 0;
  analogWrite(_pina, 0 );             
  analogWrite(_pinb, 0 );
}
//-----------------------------------------------------

byte normalSpeed = 160; // General speed
byte spinSpeed   =  40; // Spins Speed

//aanmaken van 4 wiel objecten
Wheel Motor1LA( 5, 6); // Links Achter
Wheel Motor2LV( 2, 3); // Links voor
Wheel Motor3RA( 9,10); // Rechts Achter
Wheel Motor4RV(12,13); // Rechts voor

void setup()
{ Serial.begin(9600);
}

void loop()
{
  if (Serial.available() > 0)
  { switch( Serial.read() )
    { case 'a': // ------------------- Drive Forward ----------------
        Motor1LA.forward(normalSpeed);
        Motor3RA.forward(normalSpeed);
        break;
      case 'd': // ------------------- Drive Backward ----------------
        Motor1LA.backward(normalSpeed);
        Motor3RA.backward(normalSpeed);
        break;
      case 'w': // ------------------- Turn Right     ----------------
        Motor1LA.forward(spinSpeed);    Motor2LV.forward(spinSpeed);
        Motor3RA.backward(spinSpeed);   Motor4RV.backward(spinSpeed);
      break;
      case 's': // ------------------- Turn Left      ----------------
        Motor1LA.backward(spinSpeed);   Motor2LV.backward(spinSpeed);
        Motor3RA.forward(spinSpeed);    Motor4RV.forward(spinSpeed);
      break;
      case 'q':  // ------------------- Stop          ----------------
        Motor1LA.idle();                Motor2LV.idle();       
        Motor3RA.idle();                Motor4RV.idle();
      break;
   }//switch
  }//if
}//loop


Code compileerd goed, echter nog niet getest, dus als iemand nog typo's ziet ?.

Volgende stap is dan eigenlijk een 'Vehicle' class aan te maken , een Vehicle object bestaat dan uit 4x wheel object + extra functioneel gedrag.
Dan wordt de software zeer leesbaar bv: Vehicle.forward(100); of Vehicle.right();

[astrofrostbyte Profiel Privébericht]

Weer ff om te leren, heb ik een Vehicle (voertuig) class gemaakt, deze maakt gebruik van 4x de Wheel class , 4 wielen dus.

Hieronder de Vehicle class, de constructor was wel ff moeilijk, moet al een referentie naar de Wheel classes maken blijkbaar

Code: Alles selecteren

class Vehicle
{ public:
    Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh);
    void forward(byte spin);
    void backward(byte spin);
    void turnright(byte spin);
    void turnleft(byte spin);
    void idle();   
  private:
    byte _spin;
    byte ScaleSpeed(byte in);
    Wheel LeftAft ; Wheel LeftFor;
    Wheel RightAft; Wheel RightFor;
};

Vehicle::Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh)
        : LeftAft(pina,pinb)  , LeftFor(pinc,pind)
        , RightAft(ping,pinh) , RightFor(pind,pine)
{ _spin = 0;
}

void Vehicle::forward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);
  RightAft.forward(_spin);
}
void Vehicle::backward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);
  RightAft.backward(_spin);
}
void Vehicle::turnright(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);     LeftFor.forward(_spin);
  RightAft.backward(_spin);   RightFor.backward(_spin);
}
void Vehicle::turnleft(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);    LeftFor.backward(_spin);
  RightAft.forward(_spin);    RightFor.forward(_spin);
}
void Vehicle::idle()
{ _spin = 0;
  LeftAft.idle();             LeftFor.idle();       
  RightAft.idle();            RightFor.idle();}
byte Vehicle::ScaleSpeed(byte in)
{   int s = ( (255*in) / 100);
    if ( s > 255)
      return ( 255 );
    return ( (byte)s );
}

Nu kan je in de code een voertuig aanmaken,
Vehicle Robot(2,3, 5,6, 9,10, 12,13); //aanmaken van robot object bestaande uit 4 wiel objecten


Volledige Robot code die gebruik maakt van de reeds gemaakt libraries:
Het compileerd natuurlijk wel, maar ik heb het niet kunnen testen.

Code: Alles selecteren

//-----------------------------------------= Wheel Library =----
class Wheel
{ public:
    Wheel(int pina, int pinb);
    void forward(byte spin);
    void backward(byte spin);
    void idle();
  private:
    int  _pina;
    int  _pinb;
    byte _spin;
};

Wheel::Wheel(int pina, int pinb)
{ _pina = pina;
  _pinb = pinb;
  _spin = 0;
}
void Wheel::forward(byte spin)
{ _spin = spin;
  analogWrite(_pina, 0    );             
  analogWrite(_pinb, _spin);
}
void Wheel::backward(byte spin)
{ _spin = spin;
  analogWrite(_pina, _spin);             
  analogWrite(_pinb, 0    );
}
void Wheel::idle()
{ _spin = 0;
  analogWrite(_pina, 0 );             
  analogWrite(_pinb, 0 );
}
//--------------------------------------------= Vehicle Library =----------

class Vehicle
{ public:
    Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh);
    void forward(byte spin);
    void backward(byte spin);
    void turnright(byte spin);
    void turnleft(byte spin);
    void idle();   
  private:
    byte _spin;
    byte ScaleSpeed(byte in);
    Wheel LeftAft ; Wheel LeftFor;
    Wheel RightAft; Wheel RightFor;
};

Vehicle::Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh)
        : LeftAft(pina,pinb)  , LeftFor(pinc,pind)
        , RightAft(ping,pinh) , RightFor(pind,pine)
{ _spin = 0;
}

void Vehicle::forward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);
  RightAft.forward(_spin);
}
void Vehicle::backward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);
  RightAft.backward(_spin);
}
void Vehicle::turnright(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);     LeftFor.forward(_spin);
  RightAft.backward(_spin);   RightFor.backward(_spin);
}
void Vehicle::turnleft(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);    LeftFor.backward(_spin);
  RightAft.forward(_spin);    RightFor.forward(_spin);
}
void Vehicle::idle()
{ _spin = 0;
  LeftAft.idle();             LeftFor.idle();       
  RightAft.idle();            RightFor.idle();}
byte Vehicle::ScaleSpeed(byte in)
{   int s = ( (255*in) / 100);
    if ( s > 255)
      return ( 255 );
    return ( (byte)s );
}

//-----------------------------------------------------

byte normalSpeed =  80; // General speed 80%
byte spinSpeed   =  20; // Spins  Speed 20%

//aanmaken van robot object bestaande uit 4 wiel objecten
Vehicle Robot(2,3, 5,6, 9,10, 12,13);

void setup()
{ Serial.begin(9600);
}

void loop()
{
  if (Serial.available() > 0)
  { switch( Serial.read() )
    { case 'a': // ------------------- Drive Forward ----------------
        Robot.forward(normalSpeed);
        break;
      case 'd': // ------------------- Drive Backward ----------------
        Robot.backward(normalSpeed);
        break;
      case 'w': // ------------------- Turn Right     ----------------
        Robot.turnright(spinSpeed);
      break;
      case 's': // ------------------- Turn Left      ----------------
        Robot.turnright(spinSpeed);
        break;
      case 'q':  // ------------------- Stop          ----------------
        Robot.idle();
        break;
   }//switch
  }//if
}//loop


ps. als ik een keer tijd vindt kan ik de code wel testen met de 'Open Log Sniffer' logic analyser

[astrofrostbyte Profiel Privébericht]

Beste Astrofrostbye,
Ben eigenlijk nog een leek op arduino gebied en heb na een paar uur het naar voor en het naar achter rijden onder de knie gekregen om een code te maken. Alleen zou u mij kunnen helpen om het naar links te sturen en het naar rechts sturen te kunnen herschrijven ? Als het voertuig naar links moet dan moeten moter 1 en 4 draaien moter 1 moet dan naar achter draaien en moter 4 naar voren en dan als het voertuig naar rechts moet dan moet moter 3 naar achter en moter 2 naar voren. Hoe kan ik dat in de bestaande code verwerken en zou u me kunnen helpen ?

De eerste stap zou zijn de code wat leesbaarder te maken, iets wat ik eigenlijk aan het doen was voor mijn eigen leerprocess door de Libraries te maken.
Als de Pinnummers duidelijke namen hebben, en de functies duidelijke namen hebben is het al een stuk makkelijker om de code te maken.

En het basis probleem dat je hebt/had in de code is dat de aangegeven snelheden onder de 255 moeten blijven. Dus als je die twee snelheden aanpast doet hij het mischien al.


Om het geheel een beetje te testen zou het verstandig zijn om een paar commando's toe te voegen in je switch-case bv:
case '1': // motor 1 forward
case '2': // motor 2 forward
case '3': // motor 3 forward
case '4': // motor 4 forward
case '5': // motor 1 backward
.... enz ....
etc. dan kan je teminste goed het gedrag testen

[astrofrostbyte Profiel Privébericht]

Ik heb de code getest mbv de logic analyser.
Wat ik nog wel aangepast heb is dat in forward en backward al de vier wielen bekrachtigd zijn.

In het onderstaande plaatje zie je een sequentie van commando's (recht terminal).


De code, heb ik getest met een Arduino Due, maar met een Mega zou het ook moeten lukken denk ik, een normale Uno heeft te weinig analogOut pinnen.

Code: Alles selecteren

//-----------------------------------------= Wheel Library =----
class Wheel
{ public:
    Wheel(int pina, int pinb);
    void forward(byte spin);
    void backward(byte spin);
    void idle();
  private:
    int  _pina;
    int  _pinb;
    byte _spin;
};

Wheel::Wheel(int pina, int pinb)
{ _pina = pina;
  _pinb = pinb;
  idle();
  _spin = 0;
}
void Wheel::forward(byte spin)
{ _spin = spin;
  analogWrite(_pina, 0    );             
  analogWrite(_pinb, _spin);
}
void Wheel::backward(byte spin)
{ _spin = spin;
  analogWrite(_pina, _spin);             
  analogWrite(_pinb, 0    );
}
void Wheel::idle()
{ _spin = 0;
  analogWrite(_pina, 0 );             
  analogWrite(_pinb, 0 );
}
//--------------------------------------------= Vehicle Library =----------

class Vehicle
{ public:
    Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh);
    void forward(byte spin);
    void backward(byte spin);
    void turnright(byte spin);
    void turnleft(byte spin);
    void idle();   
  private:
    byte _spin;
    byte ScaleSpeed(byte in);
    Wheel LeftAft ; Wheel LeftFor;
    Wheel RightAft; Wheel RightFor;
};

Vehicle::Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh)
        : LeftAft(pina,pinb)  , LeftFor(pinc,pind)
        , RightAft(pine,pinf) , RightFor(ping,pinh)
{ _spin = 0;
}

void Vehicle::forward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);     LeftFor.forward(_spin);
  RightAft.forward(_spin);    RightFor.forward(_spin);
}
void Vehicle::backward(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);    LeftFor.backward(_spin);
  RightAft.backward(_spin);   RightFor.backward(_spin);
}
void Vehicle::turnright(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.forward(_spin);     LeftFor.forward(_spin);
  RightAft.backward(_spin);   RightFor.backward(_spin);
}
void Vehicle::turnleft(byte spin)
{ _spin = ScaleSpeed(spin);
  LeftAft.backward(_spin);    LeftFor.backward(_spin);
  RightAft.forward(_spin);    RightFor.forward(_spin);
}
void Vehicle::idle()
{ _spin = 0;
  LeftAft.idle();             LeftFor.idle();       
  RightAft.idle();            RightFor.idle();}
byte Vehicle::ScaleSpeed(byte in)
{   int s = ( (255*in) / 100);
    if ( s > 255)
      return ( 255 );
    return ( (byte)s );
}

//-----------------------------------------------------

byte normalSpeed =  80; // General speed 80%
byte spinSpeed   =  20; // Spins  Speed 20%

//aanmaken van robot object bestaande uit 4 wiel objecten
Vehicle Robot(5,6, 2,3, 12,13, 9,10); // (LA,LV,RV,RA)
// Motor 1 Links Achter  Pin  5,6
// Motor 2 Links Voor    Pin  1,2
// Motor 3 Rechts Achter Pin  9,10
// Motor 4 Rechts Voor   Pin  12,13

void setup()
{ Serial.begin(9600);
 // TestAnalogwrite();
  Serial.println("Robot Vehicle");
}

//void TestAnalogwrite()
//{
//  analogWrite(2,64);   analogWrite(3,192); delay(30)  ;
//  analogWrite(2, 0);   analogWrite(3,  0); delay(10)  ; 
//  analogWrite(5,64);   analogWrite(6,192); delay(30)  ;
//  analogWrite(5, 0);   analogWrite(6,  0); delay(10)  ; 
//  analogWrite(9,64);   analogWrite(10,192); delay(30)  ;
//  analogWrite(9, 0);   analogWrite(10,  0); delay(10)  ; 
//  analogWrite(12,64);   analogWrite(13,192); delay(30)  ;
//  analogWrite(12, 0);   analogWrite(13,  0); delay(10)  ; 
//}

void loop()
{
  if (Serial.available() > 0)
  { switch( Serial.read() )
    { case 'a': // ------------------- Drive Forward ----------------
        Serial.println("DriveForward");
        Robot.forward(normalSpeed);
        break;
      case 'd': // ------------------- Drive Backward ----------------
        Serial.println("DriveBackward");
        Robot.backward(normalSpeed);
        break;
      case 'w': // ------------------- Turn Right     ----------------
        Serial.println("TurnRight");
        Robot.turnright(spinSpeed);
      break;
      case 's': // ------------------- Turn Left      ----------------
        Serial.println("TurnLeft");       
        Robot.turnleft(spinSpeed);
        break;
      case 'q':  // ------------------- Stop          ----------------
        Serial.println("Idle");
        Robot.idle();
        break;
   }//switch
  }//if
}//loop


Er zijn ook twee manieren om te draaien, de huidige manier in de code , en wat ook nog kan is bv. de linker wielen langzamer laten draaien dan de rechter wielen.

Wat als uitbreiding gedaan zou kunnen worden is dat als het :
- voertuig stil staat dat je dan op de plek draait, met bv linker wielen achterwaards, rechter wielen voorwaards.
- Als het voertuig rijd, dat dan de linker of rechter wielen langzamer gezet worden. een bocht maken dus.

[Pronkie Profiel Privébericht]

Iedereen onwijs bedankt voor de hulp en de moeite om mij te helpen heb er een hoop van geleerd en kan nu vooruit en achteruit en naar links draaien rechts moet ik nog even aanpassen. de volgende stap is dat ik hem via me pc via bluetooth kan besturen met de zelfde wsda toetsen als die ik nu in de code gebruik.

Mvg Anthony

[Pronkie Profiel Privébericht]

Hallo,
De robot werkt nu zoals ik wil dat die werkt na nog wat dingen te hebben aangepast en na wat kleinen bugs eruitgehaald te hebben maar nu wil ik als die naar links gaat rijden een 2 ledjes laten knipperen een rood ledje en een wit ledje allebei op een andere input en voor recht het zelfde maar heb al een aantal dingen geprobeerd maak kom er niet uit kan iemand mij uitleggen hoe je de code moet schijven zodat je hem via je serial monitor kan besturen zoals ik nu doe. heb de inputs 8 7 4 en 11 nog over. Ze moeten allemaal op een andere input komen zodat ik hem dan later kan uitbreiden met naar voren rijden wit licht links en rechts en naar achter rijden rood licht links en rechts

Zie code

Code: Alles selecteren

Input 7
Input 8
Input 4
Input 11

Als a ingedrukt wordt moeten led wit LV en rood LA gaan branden
Als d ingedrukt wordt moeten led Wit RV en rood RA gaan branden
Als w ingedrukt wordt moeten de witten leds RV en LV gaan branden
Als s ingedrukt wordt moeten de roden leds LA en RA gaan branden

Input 4 is LV
Input 7 is RV
Input 8 is LA
Input 11 is RA

toets w is robot rijdt naar voren
toets s is robot rijdt naar achter
toets a is robot rijdt naar links
toets d is robot rijdt naar rechts

Code voor het knipperen naar rechts

int ledPin = 7,11
void setup() {
pinMode(ledPin, OUTPUT);
}
void loop() {
digitalWrite(ledPin, HIGH);
delay(1000);
digitalWrite(ledPin, LOW);
delay(1000);
}

Code voor het knipperen naar links

int ledPin = 4,8
void setup() {
pinMode(ledPin, OUTPUT);
}
void loop() {
digitalWrite(ledPin, HIGH);
delay(1000);
digitalWrite(ledPin, LOW);
delay(1000);
}

Code voor het branden van de led naar als de robot naar voren gaat

int ledPin = 4,7
void setup() {
pinMode(ledPin, OUTPUT);
}
void loop() {
digitalWrite(ledPin, HIGH);

}

Code voor het branden van de led naar als de robot naar achter gaat

int ledPin = 8,11
void setup() {
pinMode(ledPin, OUTPUT);
}
void loop() {
digitalWrite(ledPin, HIGH);

}


mvg anthony

[astrofrostbyte Profiel Privébericht]

Hang de led aansturing in het zelfde switch-case blok waar je de "a,s,w,d " afhandeld,
Zet bv. bij een "W" commando de voorwaardse LED's aan en zet alle andere leds weer uit.
De delay() functie voor het knipperen heeft veel nadelen, delay() zorgt er bv. ook voor dat je communicatie niet afgehandeld wordt.

Kijk voor het laten knipperen van een led eens naar het standaard voorbeeld in Arduino : Bestand->Voorbeelden->2.0 Digital->BlinkWithoutDelay


ook ff aan de vehicle library toegevoegd,

Code: Alles selecteren

  //-----------------------------------------= Wheel Library =----
  class Wheel
  { public:
      Wheel(int pina, int pinb);
      void forward(byte spin);
      void backward(byte spin);
      void idle();
    private:
      int  _pina;
      int  _pinb;
      byte _spin;
  };
 
  Wheel::Wheel(int pina, int pinb)
  { _pina = pina;
    _pinb = pinb;
    idle();
    _spin = 0;
  }
  void Wheel::forward(byte spin)
  { _spin = spin;
    analogWrite(_pina, 0    );             
    analogWrite(_pinb, _spin);
  }
  void Wheel::backward(byte spin)
  { _spin = spin;
    analogWrite(_pina, _spin);             
    analogWrite(_pinb, 0    );
  }
  void Wheel::idle()
  { _spin = 0;
    analogWrite(_pina, 0 );             
    analogWrite(_pinb, 0 );
  }
  //--------------------------------------------= Vehicle Library =----------
 
  class Vehicle
  { public:
      Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh,int LedLA,int LedLV, int LedRV, int LedRA);
      void forward(byte spin);
      void backward(byte spin);
      void turnright(byte spin);
      void turnleft(byte spin);
      void idle();
      void heartbeat();     
    private:
      byte _spin;
      byte ScaleSpeed(byte in);
      int _LedLA, _LedLV, _LedRV, _LedRA;
      boolean _LedBlink;
      unsigned int _LedBlinkSpeed;
      unsigned long TimeStamp;
      Wheel LeftAft ; Wheel LeftFor;
      Wheel RightAft; Wheel RightFor;
  };
 
  Vehicle::Vehicle(int pina,int pinb,int pinc,int pind,int pine,int pinf,int ping,int pinh,int LedLA, int LedLV, int LedRV, int LedRA)
          : LeftAft(pina,pinb)  , LeftFor(pinc,pind)
          , RightAft(pine,pinf) , RightFor(ping,pinh)
  { _spin = 0;
    _LedLA = LedLA           ;  _LedLV = LedLV ;
    _LedRA = LedRA           ;  _LedRV = LedRV ;
    _LedBlink = false;
    _LedBlinkSpeed = 200;
    digitalWrite(_LedLA, LOW);  digitalWrite(_LedLV, LOW);
    digitalWrite(_LedRA, LOW);  digitalWrite(_LedRV, LOW);
  }
  void Vehicle::forward(byte spin)  //-------------------------
  { _spin = ScaleSpeed(spin);
    LeftAft.forward(_spin);     LeftFor.forward(_spin);
    RightAft.forward(_spin);    RightFor.forward(_spin);
    digitalWrite(_LedLA, LOW);  digitalWrite(_LedLV, HIGH);
    digitalWrite(_LedRA, LOW);  digitalWrite(_LedRV, HIGH);
    _LedBlink = false;
  }
  void Vehicle::backward(byte spin) //-------------------------
  { _spin = ScaleSpeed(spin);
    LeftAft.backward(_spin);    LeftFor.backward(_spin);
    RightAft.backward(_spin);   RightFor.backward(_spin);
    digitalWrite(_LedLA, HIGH);  digitalWrite(_LedLV, LOW);
    digitalWrite(_LedRA, HIGH);  digitalWrite(_LedRV, LOW);
    _LedBlink = false;
  }
  void Vehicle::turnright(byte spin)//-------------------------
  { _spin = ScaleSpeed(spin);
    LeftAft.forward(_spin);     LeftFor.forward(_spin);
    RightAft.backward(_spin);   RightFor.backward(_spin);
    digitalWrite(_LedLA, LOW);  digitalWrite(_LedLV, LOW);
    digitalWrite(_LedRA, HIGH); digitalWrite(_LedRV, HIGH);
    _LedBlink = true;
  }
  void Vehicle::turnleft(byte spin) //-------------------------
  { _spin = ScaleSpeed(spin);
    LeftAft.backward(_spin);    LeftFor.backward(_spin);
    RightAft.forward(_spin);    RightFor.forward(_spin);
    digitalWrite(_LedLA, HIGH); digitalWrite(_LedLV, HIGH);
    digitalWrite(_LedRA, LOW);  digitalWrite(_LedRV, LOW);
    _LedBlink = true;
  }
  void Vehicle::idle()  //-------------------------------------
  { _spin = 0;
    LeftAft.idle();             LeftFor.idle();       
    RightAft.idle();            RightFor.idle();
    digitalWrite(_LedLA, LOW);  digitalWrite(_LedLV, LOW);
    digitalWrite(_LedRA, LOW);  digitalWrite(_LedRV, LOW);
    _LedBlink = false;
  }
  byte Vehicle::ScaleSpeed(byte in)  //-------------------------
  {   int s = ( (255*in) / 100);
      if ( s > 255 )
        return ( 255 );
      return ( (byte)s );
  }
  void Vehicle::heartbeat()          //-------------------------
  { if (_LedBlink)
    { if ( millis() >= TimeStamp )
      { TimeStamp = millis() + _LedBlinkSpeed ;
        // TODO Blink the LEDS
      }
    }
  }
  //------------------------------------------------------------
 
  byte normalSpeed =  80; // General speed 80%
  byte spinSpeed   =  20; // Spins  Speed 20%
 
  // Aanmaken van robot object bestaande uit 4 wiel objecten, en verlichting
  Vehicle Robot(5,6, 2,3, 12,13, 9,10 , 8, 4, 11, 7); // (LA,LV,RV,RA) (LED LA,LV,RV,RA)
  // Motor 1 Links Achter  Pin  5,6   , LedLA is pin 8
  // Motor 2 Links Voor    Pin  2,3   , LedLV is pin 4
  // Motor 3 Rechts Achter Pin  9,10  , LedRA is pin 11
  // Motor 4 Rechts Voor   Pin  12,13 , LedRV is pin 7
 
  void setup()
  { Serial.begin(9600);
    Serial.println("Robot Vehicle");
  }
 
  void loop()
  {
    if (Serial.available() > 0)
    { switch( Serial.read() )
      { case 'a': // ------------------- Drive Forward  ----------------
          Serial.println("DriveForward");
          Robot.forward(normalSpeed);
          break;
        case 'd': // ------------------- Drive Backward ----------------
          Serial.println("DriveBackward");
          Robot.backward(normalSpeed);
          break;
        case 'w': // ------------------- Turn Right     ----------------
          Serial.println("TurnRight");
          Robot.turnright(spinSpeed);
        break;
        case 's': // ------------------- Turn Left      ----------------
          Serial.println("TurnLeft");       
          Robot.turnleft(spinSpeed);
          break;
        case 'q': // ------------------- Stop           ----------------
          Serial.println("Idle");
          Robot.idle();
          break;
     }//switch
    }//if
    Robot.heartbeat();
  }//loop