skule.sormo.no

ORG NR 885 947 522

Arduinoprosjekter

Styring av kjøretøy med faste drivhjul

{jcomments on}Denne Arduino-skissen leser to RC-kanaler og styrer fart og sving på kjøretøy med faste drivmotorer, enten en eller to på hver side. Kjøretøyet svinger når det er ulik hastighet på de to sidene av kjøretøyet. 

Med appen 'Control Jpystick' på Android-telefonen er det muligå bruke blueooth til å kommunisere med kjøretøyet 

 

Disse kjøretøyene svinger ved å øke farten på den ene siden og minke på den andre. Med appen 'Control Joystick' på Android-telefonen er det mulig å styre farten på kjøretøyet analogt. Kommunikasjonen skjer med bluetooth med en hastighet på 115200 baud. To skyvespaker kontrollerer fart og sving.  Appen sender et sett på fire byte når det skjer endringer på styrepanelet. Det første er kode for frem (0xF1),tilbake[0xF2) eller stopp (0xF3). Det andre er en analog verdi av farten, med verdier mellom 0 og 255. Det tredje er svingretning, som styrer direte en servo dersom den svinger på hjul dersom det finnes på kjøretøyet. Midtvderdien er 88 som er rett fram. Det fjerde og siste byte-et betjener digitale verdier hvor hovedlys (0x80), baklys (0x40), funksjonsskifte (0x20) for å styre en ekstra servo som har styringskoden 0xF4 i første byte. Deretter følger lydhord (0x10) og tre ekstrafunksjoner(0x08, 0x04 og 0x02). Appen leser også verdiene på analogportene som mottas fra Arduinokortet.  På en nettside for appen er det gitt en Arduinoskisse for å styre med bil der en servo er brukt for å svinge på forhjulene.  Dette er modifisert ut fra den første skissen under for å styre et kjøretøy der det svinger ved å gi ulik hastighet på hjulene på hver side. Skissen ble først testet på MEGA. Der var det mulig å bruke to serieporter der bluetooth-enheten ble koblet til Serial1. Dette gjorde det mulig å utvikle og teste skissen via USB-porten. Ulike utskriftsvariable ble sendt til PC for kontroll mens bluetooth-kommunikasjonen skjedde via Serial1. Når alt virket som det skulle, ble alle utskrifter som ikke skal være med fjernet og testet på et UNO-kort. Deretter ble det hele loddet opp på et Arduino mini pro-kort med et HC-06 bluetoothkort. 
Appen har også en funksjon der du kan helle på telefonen framover, tilbake og til en av sidene for å styre kjøretøyet.
Digitaportene 5 og 6 brukes til å styre motoren fram og tilbake. Portene 9 0g 10 brukes til svingfunksjonen. Alle disse fire portene haer pulsbreddemodulasjon, og det er dette som brukes for å styre de analoge verdiene til motorene.
 

 

 

 /* Denne rutinen leser to kanaler fra en RC-mottaker og styrer et kjøretøy med   faste drivhjul, enten to på hver side eller en på hver side. Kjøretøyet svinger når hjulene på sidene har ulik hastighet.
(C) Skule Sørmo 2014
*/


#define M1a 5   // pwm-porter
#define M1b 6
#define M2a 9
#define M2b 10
#define ENA 8
#define ENB 7
#define horn 4

void setup()
{
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
analogWrite(M1a,0);  // kjøretøyet holdes i ro
analogWrite(M1b,0);
analogWrite(M2a,0);
analogWrite(M2b,0);
digitalWrite(ENA,HIGH);
digitalWrite(ENB,LOW);
}

void loop()
{
 int ch1 = pulseIn(A0, HIGH, 25000); // Read the pulse width of 
 int ch2 = pulseIn(A1, HIGH, 25000); // each channel 

  ch1=constrain(ch1,970,1770);
  ch2=constrain(ch2,970,1770);

// konverterer  RC-verdier til  motorverdier
  int x = map(ch1, 970, 1770, -255, 255);   //fart
  int y = map(ch2, 970, 1770, -125,125);   //sving

  analogWrite(M1a,constrain(x-y,0,255));
  analogWrite(M1b,constrain(-x+y,0,255));
  analogWrite(M2a,constrain(x+y,0,255));
  analogWrite(M2b,constrain(-x-y,0,255));
}

Her er en programskisse som styrer et kjøretøy med faste hjulsett bved bruk av appen 'Control Joystick':

#include <Servo.h>
//#include <L293.h>
#define pinForward 8     
#define pinBack 7       
#define pinSpeedForwardBack 6
#define pinFrontLights 2
#define pinBackLights 3
#define pinFrontSteering 10
//L293(pinForward, pinBack, pinFwdBakVel);
//L293 redCar(pinForward,pinBack,pinSpeedForwardBack);
Servo leftRight;
byte commands[4] = {0x00,0x00,0x00,0x00};
byte prevCommands[4] = {0x01,0x01,0x01,0x01};
//Variables will be used to determine the frequency at which the sensor readings are sent 
//to the phone, and when the last command was received.  
unsigned long timer0 = 2000;  //Stores the time (in millis since execution started) 
unsigned long timer1 = 0;  //Stores the time when the last sensor reading was sent to the phone
unsigned long timer2 = 0;  //Stores the time when the last command was received from the phone
//14 byte payload that stores the sensor readings
byte three[14] = {0xee,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xcc};
//Constant used to caculate the 9V battery voltage (9.04 mV per step)
float stepSize = 9.04;
//The union allows you to declare a customized data type, in this case it can be either 
//a float or a byte array of size 4. What we need is to store a float which is 4
//bytes long and retrieve each of the 4 bytes separately.
union u_sensor0{
  byte a[4];  
  float b;
}sensor0;
union u_sensor1{
  byte c[4];  
  float d;
}sensor1;
int i = 0;

 #define M1a 5   // pwm-porter
 #define M1b 6
 #define M2a 9
 #define M2b 10
 #define ENA 8
 #define ENB 7

#define bluetooth Serial

void setup()
{       
  bluetooth.begin(115200);  
//  Serial.begin(115200);  
//  Serial.println("test");
  pinMode(pinFrontLights, OUTPUT);
  pinMode(pinBackLights, OUTPUT);
  digitalWrite(pinFrontLights,HIGH);
  digitalWrite(pinFrontLights,HIGH);
 // delay(500);
  digitalWrite(pinFrontLights,LOW);
//  digitalWrite(pinFrontLights,LOW);

  motor(0,88); // kjøretøy i roø Første parameter er fart, den andre retning der 88 er rett fram
}

void loop()
{
  if (bluetooth.available() == 4) {
    timer2 = millis();  //Store the time when last command was received
    memcpy(prevCommands, commands, 4); //Storing the received commands
    commands[0] = bluetooth.read();  //Direction
    commands[1] = bluetooth.read();  //Speed
    commands[2] = bluetooth.read();  //Angle
    commands[3] = bluetooth.read();  //Lights and buttons states
/*
    Serial.print(int(commands[0]), HEX);
    Serial.print("  ");
    Serial.print(int(commands[1]));
    Serial.print("  ");
    Serial.print(int(commands[2]));
    Serial.print("  ");
    Serial.print(int(commands[3]));
    Serial.println("  ");
*/
 if((commands[2]<=0xb4)&&((commands[0]<=0xf5)&&(commands[0]>=0xf1))){
      //Make sure that the command received involves controlling the car's motors (0xf1,0xf2,0xf3)
      if(commands[0] <= 0xf3){
        if(commands[0] == 0xf1){  //Check if the move forward command was received
          if(prevCommands[0] != 0xf1){  //Change pin state to move forward only if previous state was not move forward
//            redCar.forward_1W(commands[1]);
 //    Serial.println("Updated direction FWD");
        motor(commands[1],commands[2]);
          }  
        }
        else if(commands[0] == 0xf2){  //Check if the move back command was received     
          if(prevCommands[0] != 0xf2){  //Change pin state to move back only if previous state was not move back
 //           redCar.back_1W(commands[1]);
//Serial.println("Updated direction BAK");
           motor(-commands[1],commands[2]);
          }
        }
        else if(commands[0] == 0xf3){ //Check if the move back command was received     
//          if(prevCommands[0] != 0xf3)
             {  //Change pin state to move back only if previous state was not move back
//            redCar.stopped_1W();   
//Serial.println("Updated direction STP");
          motor(0,commands[2]);
          }
        }
        //Change speed only if new speed is not equal to the previous speed
        if(prevCommands[1] != commands[1]){
          prevCommands[1]=commands[1];
//          redCar.setSpeed_1W(commands[1]);  
//Serial.println("Updated speed");
        if(commands[0] == 0xf1)
           motor(commands[1],commands[2]);
         else if(commands[0] == 0xf2)
           motor(-commands[1],commands[2]);
       } ;    
        //Steer front wheels only if the new angle is not equal to the previous angle
        if(prevCommands[2] != commands[2]){
          leftRight.write(commands[2]);  
          //Serial.println("Updated angle"); 
        }         
      }
      else if(commands[0] == 0xf5){
        if(prevCommands[0] != 0xf5){
          //Stop everything
//          redCar.stopped_1W(); 
      motor(0,commands[2]);
          digitalWrite(pinFrontLights,LOW);
          digitalWrite(pinBackLights,LOW);
        }
      }
      else{
        //Here you put the code that will control the tilt pan (commands[0] == 0xf4)   
      } 
    //Check the front/back lights and other toggles   
      if(prevCommands[3] != commands[3]){
//Serial.println(commands[3],BIN);
      //Change the light/button states
        //               _______________________________________________
        //command[3] =  |  0  |  0  |  0  |  0  |  0  |  0  |  0  |  0  |  binary
        //              |_____|_____|_____|_____|_____|_____|_____|_____|
        //Buttons ---->  Front  Back  Horn   A     B     C     D     E   
        //Front lights
        if((bitRead(prevCommands[3],7))!=(bitRead(commands[3],7))){
          if(bitRead(commands[3],7)){
            digitalWrite(pinFrontLights,HIGH);
          }
          else{
            digitalWrite(pinFrontLights,LOW);
          }
        }
        //Back lights
        if((bitRead(prevCommands[3],6))!=(bitRead(commands[3],6))){
          if(bitRead(commands[3],6)){
            digitalWrite(pinBackLights,HIGH);
          }
          else{
            digitalWrite(pinBackLights,LOW);
          }
        }
        //Horn
        if((bitRead(prevCommands[3],5))!=(bitRead(commands[3],5))){
          if(bitRead(commands[3],5)){
            tone(horn,500);
          }
          else{
            noTone(horn);  // slår av hornet
          }

        }
      }      
    }
    else
    {
      //Resetting the Serial port (clearing the buffer) in case the bytes are not being read in correct order.
//      Serial.end();
//      Serial.begin(9600);
    }
  }
  else{
    timer0 = millis();  //Get the current time (millis since execution started)
    if((timer0 - timer2)>400){  //Check if it has been 400ms since we received last command
      //More tan 400ms have passed since last command received, car is out of range. Therefore
      //Stop the car and turn lights off
//    redCar.stopped_1W();  
 //     motor(0,commands[2]);
      digitalWrite(pinFrontLights,LOW); 
      digitalWrite(pinBackLights,LOW);
    }
    if((timer0 - timer1)>=477){  //Check if it has been 477ms since sensor reading were sent
      //Calculate the 9V's voltage by multiplying the step size by the step number (analogRead(0)) 
      //This value will be in mV, which is why it's multiplied by 0.001 to convert into Volts.
      sensor0.b = (analogRead(0) * stepSize) * 0.001;  
      //Break the sensor0 float into four bytes for transmission
      three[1] = sensor0.a[0];
      three[2] = sensor0.a[1];
      three[3] = sensor0.a[2];
      three[4] = sensor0.a[3];
      //Get sensor 2's reading
      sensor1.d = analogRead(1);  
      //Break the sensor1 float into four bytes for transmission
      three[5] = sensor1.c[0];
      three[6] = sensor1.c[1];
      three[7] = sensor1.c[2];
      three[8] = sensor1.c[3];
      //Get the remaining reading from the analog inputs
      three[9] = map(analogRead(2),0,1023,0,255);
      three[10] = map(analogRead(3),0,1023,0,255);
      three[11] = map(analogRead(4),0,1023,0,255);
      three[12] = map(analogRead(5),0,1023,0,255); 
      //Send the six sensor readings
     Serial.write(three,14);
      //Store the time when the sensor readings were sent
      timer1 = millis();
    }
  }
}


void motor(int x, int y)   /
{
    y= map(y,66,110,-100,100);   // kursen endres til et intervall mellom -100 og 100. Større                                                        //verdier gir for kraftige utslag.
    analogWrite(M1a,constrain(x+y,0,255));
   analogWrite(M1b,constrain(-x-y,0,255));
   analogWrite(M2a,constrain(x-y,0,255));
   analogWrite(M2b,constrain(-x+y,0,255));
}

jj