We are building a caterpillar Bluetooth robot with a camera. Part 3

    In the previous series:
    Part 1
    Part 2
    Well, has everyone already ordered spare parts and assembled robots? It's time for the robot to revive.
    Today we will analyze the software.
    The option that I offer is as simple as possible . Do not expect unique abilities from him. His task is just to go to work. Fault tolerance, smooth control and additional functions - this is the scope for creativity, which I leave to everyone, so as not to deprive this pleasure. The code is very simple and therefore far from optimal and not secure and generally not beautiful. If there are suggestions for improving it - offer your own options, directly pieces of code explaining why and why it would be better.
    Unconstructive criticism of what has been done poorly is not particularly needed :) I already know about the shortcomings. But if something is not clear, ask, I will explain.
    So let's go!


    Let's divide the task into simple steps: On-
    board controller
    • Controlling caterpillar motors for forward / reverse and cornering
    • Camera Servo Control
    • Receiving via bluetooth and executing motion commands and controlling camera servos

    PC / Laptop
    • Calculation of the speed of motors for setting the direction of movement
    • Transfer control packets via bluetooth
    • Joystick connection for easy control

    Controlling caterpillar motors for forward / reverse and cornering


    Since we use a ready-made MotorShield and not a bare H-bridge or L293D / L298N, there is nothing particularly complicated to invent. We vspolzuemsya library AFMotor . If you have a Motorshield V3 and you need an SPI bus, take the modified version .
    I usually write comments in English, as it’s simpler and shorter.
    Declare variables for controlling motors. the right motor is connected to the 4th port, and the left motor to the 3rd port.
    AF_DCMotor rMotor(4);  //Right motor
    AF_DCMotor lMotor(3);  //Left motor


    depending on the given direction and speed, we command the motors to rotate (for the left motor):
    
          switch (lDirection){
            case 0:
              lMotor.run(RELEASE);
              break;
            case 1:
              lMotor.run(FORWARD);
              lMotor.setSpeed(lSpeed);
              break;
            case 2:
              lMotor.run(BACKWARD);
              lMotor.setSpeed(lSpeed);
              break;
          } 
    


    For the right engine the same:
    
          switch (rDirection){
            case 0:
              rMotor.run(RELEASE);
              break;
            case 1:
              rMotor.run(FORWARD);
              rMotor.setSpeed(rSpeed);
              break;
            case 2:
              rMotor.run(BACKWARD);
              rMotor.setSpeed(rSpeed);
              break;
          }
    

    lDirection (or rDirection) takes the following values:
    0 - stop the motor
    1 - forward rotation
    2 - reverse rotation.

    Camera Servo Control


    To control the servos, we declare two objects panServo (responsible for the rotation of the camera) and tiltServo (responsible for the tilt). Since the servo is mechanical and does not rotate instantly, we introduce a variable for the delay required by the drive to work out the rotation command (15 ms is enough)
    Servo panServo, tiltServo;
    long interval = 15;    // interval at which to control servo
    long previousMillis = 0;   
    unsigned long currentMillis;
    


    previousMillis and currentMillis are used to avoid waiting stupidly in the control loop when the servo runs. We check - if 15 ms has not passed since the last command, then it is useless to command the servo - it is still busy.
    Piece responsible for the rotation of the camera:
          //Rotate camera
          currentMillis = millis();
          if(currentMillis - previousMillis > interval) {
            previousMillis = currentMillis;
            if (lastPan!=pan) panServo.write(pan);	     // tell pan servo to go to position
            if (lastTilt!=tilt) tiltServo.write(tilt);   // tell tilt servo to go to position
            lastPan=pan;
            lastTilt=tilt;
          }
    

    Receiving via bluetooth and executing motion commands and controlling camera servos


    The bluetooth module from the point of view of the Arduino is simply a serial (UART) port.
    Therefore, we will interrogate in a cycle to check whether something has come from the computer. If something is found in the buffer, then we are looking for the beginning of the packet in the stream - the $ FF byte (the extreme positions of the servos and the motor speed values ​​of 255 are practically useless - the servants abut earlier, and the speed of 250-255 does not differ, so this value will occur extremely rarely and this will allow us to catch the beginning of the package, we can increase reliability by complicating the algorithm, but this is quite enough for us).
    Having found the header, we accept a byte in which the direction of the engines is encoded in 2 bits per engine. Then we read the speed of the engines - 1 byte per engine (lSpeed, rSpeed) and the position of the camera's servo drives (pan, tilt).
      if (Serial.available()>0) {
        Header=Serial.read();
        //If header found then get and process Cmd
        if (Header==255){
          while(Serial.available()<5){};
          Direction=Serial.read();
          lSpeed=Serial.read();
          rSpeed=Serial.read();
          pan=Serial.read();
          tilt=Serial.read();
    

    Next, we highlight the directions for the right and left engines
          lDirection=Direction & 0x03;
          rDirection=(Direction & 0x0C) >> 2;
    

    and if the direction or speed has changed since the last command received, then set the speed of the engines and rotate the camera.
    This is the whole main work cycle:
    void loop() {
      if (Serial.available()>0) {
        Header=Serial.read();
        //If header found then get and process Cmd
        if (Header==255){
          while(Serial.available()<5){};
          Direction=Serial.read();
          lSpeed=Serial.read();
          rSpeed=Serial.read();
          pan=Serial.read();
          tilt=Serial.read();
          lDirection=Direction & 0x03;
          rDirection=(Direction & 0x0C) >> 2;
          //Left
         if ((lastlDir!=lDirection) or (lastlSpeed!=lSpeed)){
          switch (lDirection){
            case 0:
              lMotor.run(RELEASE);
              break;
            case 1:
              lMotor.run(FORWARD);
              lMotor.setSpeed(lSpeed);
              break;
            case 2:
              lMotor.run(BACKWARD);
              lMotor.setSpeed(lSpeed);
              break;
          }
          lastlDir=lDirection;
          lastlSpeed=lSpeed;
         }
          //Right
         if ((lastrDir!=rDirection) or (lastrSpeed!=rSpeed)){
          switch (rDirection){
            case 0:
              rMotor.run(RELEASE);
              break;
            case 1:
              rMotor.run(FORWARD);
              rMotor.setSpeed(rSpeed);
              break;
            case 2:
              rMotor.run(BACKWARD);
              rMotor.setSpeed(rSpeed);
              break;
          }
          lastrDir=rDirection;
          lastrSpeed=rSpeed;
         }
          //Rotate camera
          currentMillis = millis();
          if(currentMillis - previousMillis > interval) {
            previousMillis = currentMillis; 
            if (lastPan!=pan) panServo.write(pan);			// tell pan servo to go to position
            if (lastTilt!=tilt) tiltServo.write(tilt);		// tell tilt servo to go to position
            lastPan=pan;
            lastTilt=tilt;
          }
        }
      }
    }
    

    As you can see, it’s almost nowhere easier :)
    You can download the sketch from the project page in Google code.

    We taught the chassis to execute commands. Now we need to learn how to send them.
    Those who are too lazy to understand programming or reluctant to install Delphi can download the compiled version

    (works with the Logitech Extreme 3D Pro joystick or the Chinese EasyTouch gamepad).
    imageimage
    With the rest we go further :)

    We will need:
    • Delphi 2010 (you can also Delphi 7, just a couple of lines need to be corrected in the project file)
    • TComPort component from the open ComPort Library (I have 4.11s installed)
    • Components TjvHIDDevice, TjvHIDDeviceController from JEDI VCL . I use v3.38, you can download fresher. Put it whole, come in handy

    Calculation of the speed of motors for setting the direction of movement


    Moving forward and backward does not cause any difficulties - just set the same left and right motor speeds and the same direction.
    For turns in motion, we introduce the concept of Steer - the value of deviation from direct movement. The engine speeds are calculated for moving forward and backward as follows:
      if Speed>0 then begin
        //Forward
        //Left/Right turn
        lSpeed:=Speed-Steer;
        rSpeed:=Speed+Steer;
        if lSpeed<0 then lSpeed:=0;
        if rSpeed<0 then rSpeed:=0;
        if lSpeed>MaxSpeed then lSpeed:=MaxSpeed;
        if rSpeed>MaxSpeed then rSpeed:=MaxSpeed;
      end else begin
        //Backward
        //Left/Right turn
        lSpeed:=Speed+Steer;
        rSpeed:=Speed-Steer;
        if lSpeed>0 then  lSpeed:=0;
        if rSpeed>0 then  rSpeed:=0;
        if lSpeed<(-MaxSpeed) then  lSpeed:=-MaxSpeed;
        if rSpeed<(-MaxSpeed) then  rSpeed:=-MaxSpeed;
      end;
    


    That is, when moving forward from the speed of the left engine, we subtract the deviation, we add to the speed of the right one. It turns out the effect of braking one of the tracks and the chassis smoothly turns, without stopping completely. When moving backward, the signs simply change.
    Well, we check whether the speed has exceeded the maximum permissible values. In particular, this is useful for those who have powered motors with voltage higher than they should - just limit the maximum speed and the motors will be intact.
    Examples of control:
    go forward - the direction to both motors "1", the same speed
    to go back - the direction to both motors "2", the same speed
    to turn in moving left / rightset the same direction, different speeds. Turns to a side whose speed is less.
    for turning in place - the speed is the same, the direction of the motors is different - it will turn around the center.
    stop - direction “0” for both motors

    Transfer control packets via bluetooth


    When adding a bluetooth module to a PC, 2 virtual COM ports are formed - one inbound, one outbound.
    To connect to the robot, you just need to open the outgoing port. You can specify in the list of ports in bluetooth settings or by brute force method - when connected to the correct one, the program will not swear and the LED on the module stops flashing - the connection is established, we can assume that we are connected directly to the robot.
    procedure TfTank.bConnectClick(Sender: TObject);
    begin
      if Tank.Connected then begin
        Tank.Disconnect;
        bConnect.Caption:='Connect';
      end else begin
        Tank.Port:=cbPort.Text;
        Tank.Connect;
        bConnect.Caption:='Disconnect';
        MessageBeep(MB_ICONINFORMATION);
      end;
    end;
    

    For convenience, I wrote a small class called TRCTank, which implements all actions related to communication with the robot.
      TRCTank=class
      private
        fPort:string;
        ComPort:TComPort;
        Cmd, lastCmd:TControlPacket;
        fConnected:Boolean;
        function isConnected: boolean;
      protected
      public
        constructor Create;
        destructor  Destroy;override;
        procedure   Connect;
        procedure   Disconnect;
        procedure   SendCommand(lDir,left, rDir, right, pan, tilt:Byte);
        property    Port:string read fPort write fPort;
        property    Connected:boolean read isConnected;
      end;
    

    Connect and Disconnect essentially just open / close the port and check the current state so as not to try to open the open or close the closed port.

    To send a command to the robot, we form a header that the robot will catch (we have a byte with code 255). And then we write down the commands in the order that the robot is waiting for them. It turns out such a structure
      TControlPacket=record
        Header,
        Direction,
        lSpeed,     //left motor speed
        rSpeed  :Byte;//right motor speed
        pan,
        tilt    :Byte; //Camera pan & tilt
      end;
    

    In the function of sending a command, the only thing worth mentioning is the packing of directions for both engines in one byte with an offset of 2 bits. The rest is obvious.
    procedure TRCTank.SendCommand;
    begin
      if not fConnected then Exit;
      Cmd.Header:=255;
      Cmd.Direction:=lDir + rDir shl 2;
      Cmd.lSpeed:=left;
      Cmd.rSpeed:=right;
      Cmd.pan:=pan;
      Cmd.tilt:=tilt;
      if (lastCmd.Direction=Cmd.Direction) and
        (lastCmd.lSpeed=Cmd.lSpeed) and
        (lastCmd.rSpeed=Cmd.rSpeed) and
        (lastCmd.pan=Cmd.pan) and
        (lastCmd.tilt=Cmd.tilt) then Exit;
      ComPort.Write(cmd, SizeOf(cmd));
      lastCmd:=Cmd;
    end;
    


    Joystick connection for easy control


    Unfortunately, there is not much documentation on working with HID devices that is sensible on the Internet. As a result, I went through a bunch of outdated codes that send to work through the midi port or consider joysticks as a device with 2 axes and 4 buttons. This option did not suit me. There was no information on the TjvJoystick component anywhere, so I stumbled upon it by accident. It's a pity, at this point I already wrote my component :) So if you are not too lazy to figure it out, then you can use the ready-made component from JEDI VCL.
    I work with the HID device directly and analyze Report from it by byte. But all the joystick axes are available (EasyTouch has 4 of them) and all buttons (10-12 of my joysticks).
    It works like this: using the TjvHIDDeviceController component on the form, we get a list of HID devices in the system and output it to the combo box. We give the selected device to an object of the TRjoystick class by calling SelectJoystickByID (VID, PID: Word); (Selected by VendorID and ProductID - you can see them, for example, in the device manager of the system).
    The TRjoystick class performs a checkout, being able to receive reports from the joystick, decrypts the values, sets the properties of buttons and axes, and calls the handler procedure. In our program, the handler looks like this:

    procedure TfTank.OnJoyData;
    var
      Hat:THatPosition;
      CenterCamera:Boolean;
    begin
      Hat:=hCenter;
      CenterCamera:=False;
      //Easy touch joystick
      if (joyPID=6) and (joyVID=121) then begin
        scrPitch.Position:=TREasyTouchJoystick(Joy).rZ;
        scrAileron.Position:=TREasyTouchJoystick(Joy).Z;
        scrRudder.Position:=TREasyTouchJoystick(Joy).X;
        scrThrottle.Position:=TREasyTouchJoystick(Joy).Y;
        cbFire.Checked:=TREasyTouchJoystick(Joy).Btn1;
        cbAltFire.Checked:=TREasyTouchJoystick(Joy).Btn10;
        Hat:=TREasyTouchJoystick(Joy).Hat;
        CenterCamera:=TREasyTouchJoystick(Joy).Btn2;
        Speed:=Round(((TREasyTouchJoystick(Joy).rZ)-127)*2);
        Steer:=Round((TREasyTouchJoystick(Joy).Z)-127)*2;
      end;
      //Logitech Extreme 3D Pro
      if (joyPID=49685) and (joyVID=1133) then begin
        scrPitch.Position:=TRLogitechExtreme(Joy).Pitch;
        scrAileron.Position:=TRLogitechExtreme(Joy).Aileron;
        scrRudder.Position:=TRLogitechExtreme(Joy).Rudder;
        scrThrottle.Position:=TRLogitechExtreme(Joy).Throttle;
        cbFire.Checked:=TRLogitechExtreme(Joy).Btn1;
        cbAltFire.Checked:=TRLogitechExtreme(Joy).Btn2;
        Hat:=TRLogitechExtreme(Joy).Hat;
        CenterCamera:=TRLogitechExtreme(Joy).Btn1;
        Speed:=(TRLogitechExtreme(Joy).Pitch div 8)-255;   //4096 to -256..256
        Steer:=(TRLogitechExtreme(Joy).Aileron div 4)-127; //1024 to -127..128
      end;
      ApplyDeadZone(Speed,DeadX);
      ApplyDeadZone(Steer,DeadY);
      if Speed>MaxSpeed then Speed:=MaxSpeed;
      if Speed<-MaxSpeed then Speed:=-MaxSpeed;
      if Speed>0 then begin
        //Forward
        //Left/Right turn
        lSpeed:=Speed-Steer;
        rSpeed:=Speed+Steer;
        if lSpeed<0 then lSpeed:=0;
        if rSpeed<0 then rSpeed:=0;
        if lSpeed>MaxSpeed then lSpeed:=MaxSpeed;
        if rSpeed>MaxSpeed then rSpeed:=MaxSpeed;
      end else begin
        //Backward
        //Left/Right turn
        lSpeed:=Speed+Steer;
        rSpeed:=Speed-Steer;
        if lSpeed>0 then  lSpeed:=0;
        if rSpeed>0 then  rSpeed:=0;
        if lSpeed<(-MaxSpeed) then  lSpeed:=-MaxSpeed;
        if rSpeed<(-MaxSpeed) then  rSpeed:=-MaxSpeed;
      end;
      scrLeft.Position:=-lSpeed;
      scrRight.Position:=-rSpeed;
      if (cbAltFire.Checked) and (bConnect.Caption='Connect') then bConnect.OnClick(Self);
      case Hat of
        hUp: Inc(Tilt);
        hUpRight:begin
            Inc(Tilt);Dec(pan);
          end;
        hRight: Dec(pan);
        hRightDown: begin
            Dec(Pan); Dec(tilt);
          end;
        hDown: Dec(Tilt);
        hLeftDown: begin
            Inc(pan);Dec(tilt);
          end;
        hLeft: Inc(pan);
        hLeftUp: begin
            Inc(pan);Inc(tilt);
          end;
        hCenter: if CenterCamera then begin
           pan:=panCenter;
           tilt:=tiltCenter;
        end;
      end;
      //Limit Pan&Tilt range
      if panmaxPan then pan:=maxPan;
      if tilt>maxTilt then tilt:=maxTilt;
      //Show info
      lJoy.Caption:='S:'+IntToStr(Speed)+' D:'+InttoStr(Steer)+' L:'+InttoStr(lSpeed)+' R:'+InttoStr(rSpeed);
      lhat.Caption:=THatPosString[Integer(Hat)];
      //Show camera position on sliders
      scrPan.Position:=pan;
      scrTilt.Position:=tilt;
      //Send command to tank
      Command2Tank;
    end;
    

    First, we give the raw values ​​of the coordinates of the axes to the speed range -256..256 and directions -127..128.
    Since, with linear control at low speeds, the motors do not have the strength to move the robot from the place, we introduce small dead zones (empirically) - it will only move from a certain speed value. (ApplyDeadZone (Speed, DeadX); ApplyDeadZone (Steer, DeadY);)
    After taking the rudder into account, we check that the speeds have not climbed out of the range and show the speed of the motors on the form with sliders.
    Then, depending on the position of the hat, we change the direction of the camera or center it, we also check the limitations (the servos abut mechanically usually before they reach the digital control limits). display the position of the camera on another pair of sliders, display the speed and send a command to the tank.
    procedure TfTank.Command2Tank;
    begin
      lDir:=0;
      rDir:=0;
      //prepare rDir, lDir data based on tracks speed
      case  lSpeed of
       0:lDir:=0; //stop
       1..255:  lDir:=1; //forward
       -255..-1:lDir:=2; //backward
      end;
      case rSpeed of
       0:rDir:=0; //stop
       1..255:  rDir:=1; //forward
       -255..-1:rDir:=2; //backward
      end;
      Tank.SendCommand(lDir,Abs(lSpeed),rDir,Abs(rSpeed), pan, tilt);
    end;
    


    There are various experiments in the code, pieces responsible for saving the selected port and joystick, saving and loading the control and centering limits of the camera, it is possible to return control not by the joystick, but directly pulling the speed and direction sliders. But this does not concern the main task of management. You can use the code as you like, you can even “arrange BolgenOS” if you want :)
    You can download the source code of the R BT RC Tank on the project website in Google code.
    I tried to remove the control process from the first person, but shooting the screen with a camera is an ungrateful task, it came out rather mediocre. But the general meaning is clear.


    PS right now the chassis is disassembled for rework, so I can’t quickly check any changes in the code on the hardware. But the version available for download is fully functional, as seen in the video.

    Also popular now: