We build a tracked Bluetooth robot with a camera. Part 3
In the previous series:
Part 1
Part 2
Well, have everyone already ordered parts and assembled robots? It's time to revive the robot.
Today we will examine the software stuffing.
The option that I offer is as simple as possible . Do not expect unique abilities from him. His task is to justgo 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 of this pleasure. The code is very simple and therefore far from optimal and unprotected and not at all beautiful. If there are suggestions for improving it - offer your options, directly pieces of code explaining why and why it would be better.
Unconstructive criticism of what is done is bad - not really needed :) I already know about the shortcomings. But if something is not clear, ask, I will explain.
So let's go!
We divide the task into simple steps:
Onboard controller
PC / Laptop
Since we use a ready-made MotorShield and not a bare H-bridge or L293D / L298N, we will not have to invent anything particularly complicated. We will use the AFMotor library. If you have a Motorshield V3 and you need an SPI tire, take a modified version .
Out of habit, I usually write comments in English, it is simpler and shorter.
We declare variables for motor control. the right motor is connected to the 4th port, the left one to the 3rd one.
depending on the given direction and speed, let the motor rotate (for the left motor):
For the right motor is the same:
lDirection (or rDirection) takes the values:
0 - stop the motor
1 - forward rotation
2 - backward rotation.
To control the servos, we declare two panServo objects (responsible for camera rotation) and tiltServo (responsible for tilting). Since the servo is mechanical and does not rotate instantly, we introduce a variable for the delay required by the actuator to test the turn command (15 ms is enough)
')
The previousMillis and currentMillis are used so that the control loop does not stupidly wait for the servo to complete. We check - if 15 ms has not passed since the last command, then it’s useless to command the serva - it is still busy.
The piece responsible for the rotation of the camera:
Bluetooth module from the point of view of Arduino is just a serial (UART) port.
Therefore, we will be polling 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 - byte $ FF (the extreme positions of the servo drives and the speeds of the 255 engines are practically useless - the servos abut earlier, and the speed 250-255 does not differ, therefore this value Rarely and this will allow us to catch the beginning of the package, you can increase the reliability by complicating the algorithm, but this is enough for us).
Having found the header, we receive a byte in which the direction of the engines is encoded by 2 bits per engine. Then we read the engine speeds - 1 byte per engine (lSpeed, rSpeed) and the position of the camera's servos (pan, tilt).
Next, select the directions for the right and left engines
and if the direction or speed changed from the last received command, then set the speed of the engines and rotate the camera.
Here is the whole basic work cycle:
As you can see, it is practically nowhere easier :)
You can download the sketch from the project page in Google code.
We have taught the chassis to execute commands. Now we need to learn how to send them.
Who is too lazy to understand programming or reluctant to put Delphi, can download the compiled version
(works with the Logitech Extreme 3D Pro joystick or the EasyTouch Chinese gamepad).
With the rest we go further :)
We will need:
Moving forward and backward does not cause difficulties - we simply set the same speeds of the left and right motor and the same direction.
For turns in motion, we introduce the concept Steer - the deviation value from direct movement. Engine speeds are calculated for forward and backward movement as follows:
That is, when moving forward from the speed of the left engine, the deviation is subtracted, to the speed of the right engine - we add. It turns out the effect of slowing down one of the tracks and the chassis turns smoothly, without stopping completely. When moving back signs just change.
Well, we check if the speed is out of the maximum allowable values. In particular, this is useful for those who have higher voltage supply of motors than they should be - just limit the maximum speed and the motors will be intact.
Management examples:
go forward - the direction of both motors "1", the speed is the same
go back - the direction of both motors "2", the speed is the same
To rotate in moving left / right we set the same direction, the speeds are different. Turns to the side, the speed of which is less.
for turning on the spot - the speed is the same, the direction of the motors is different - it turns around the center.
stop - both motors direction "0"
When adding a bluetooth module to a PC, 2 virtual COM ports are formed - one incoming, one outgoing.
To connect to the robot you just need to open the outgoing port. You can define in the list of ports in the bluetooth settings or by the search method — when connected to the correct program, the program will not swear and the LED on the module stops blinking — the connection is established, we can assume that we are connected directly to the robot.
For convenience, I wrote a small TRCTank class that implements all actions for communicating with the robot.
Connect and Disconnect, in fact, simply 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 create a header that the robot will catch (we have a byte with the 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
In the command sending function, the only thing worth mentioning is the packing of directions for both engines in one byte by 2 bits offset. The rest is obvious.
Unfortunately, there is not much documentation on working with HID devices on the Internet. As a result, I went through a bunch of outdated codes that are sent to work through the midi port or consider the joysticks as a device with 2 axes and 4 buttons. I did not like this option. There was no information on the TjvJoystick component anywhere, so I came across it by chance. It's a pity, by this moment I have already written my component :) So if you are not too lazy to understand, you can use the ready-made component from the JEDI VCL.
I work with the HID device directly and analyze the Report from it by-byte. But all the joystick axes are available (EasyTouch has 4 of them) and all buttons (10-12 for 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 them to the combo box. The selected device is given to the object of the TRjoystick class by calling SelectJoystickByID (VID, PID: Word); (It is selected by VendorID and ProductID - they can be viewed, for example, in the system device manager).
The TRjoystick class performs checkout, being able to receive reports from the joystick, decrypts the values, sets the properties of the buttons and axes, and calls the handler procedure. In our program, the handler looks like this:
First, we bring the raw values of the axes coordinates to the speed range -256..256 and directions -127..128.
Since with linear control at low speeds, the motors do not have enough strength to move the robot, we introduce small dead zones (by experience) - it will only move from a certain speed value. (ApplyDeadZone (Speed, DeadX); ApplyDeadZone (Steer, DeadY);)
After taking into account the rudder, we check that the speeds did not get out of range, we 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, and also check the constraints (the servos rest mechanically, usually before they reach the digital limits of control). display the camera position on another pair of sliders, display the speeds and send the command to the tank.
In the code, there are still various experiments, pieces responsible for preserving the selected port and joystick, saving and loading the limits of control and centering of the camera, it is possible to return control not by the joystick, but directly by tugging at 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 :)
Download the sources for BT R RC Tank on the project’s website at Google code.
I tried to remove the process of management from the first person, but shooting the screen with a camera was a thankless task, it was rather mediocre. But the general meaning is clear.
PS right now the chassis is disassembled for rework, so I can not quickly check any changes in the code on the hardware. But the version available for download is quite workable, as seen in the video.
Part 1
Part 2
Well, have everyone already ordered parts and assembled robots? It's time to revive the robot.
Today we will examine the software stuffing.
The option that I offer is as simple as possible . Do not expect unique abilities from him. His task is to just
Unconstructive criticism of what is done is bad - not really needed :) I already know about the shortcomings. But if something is not clear, ask, I will explain.
So let's go!
We divide the task into simple steps:
Onboard controller
- Caterpillar motor control for moving forward / backward and turning
- Camera Servo Control
- Receive via bluetooth and execute motion commands and camera servo controls
PC / Laptop
- Calculate the speed of the motors to set the direction of travel
- Transmission of control packets via bluetooth
- Joystick connection for easy operation
Caterpillar motor control for moving forward / backward and turning
Since we use a ready-made MotorShield and not a bare H-bridge or L293D / L298N, we will not have to invent anything particularly complicated. We will use the AFMotor library. If you have a Motorshield V3 and you need an SPI tire, take a modified version .
Out of habit, I usually write comments in English, it is simpler and shorter.
We declare variables for motor control. the right motor is connected to the 4th port, the left one to the 3rd one.
AF_DCMotor rMotor(4); //Right motor AF_DCMotor lMotor(3); //Left motor depending on the given direction and speed, let the motor 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 motor is 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 values:
0 - stop the motor
1 - forward rotation
2 - backward rotation.
Camera Servo Control
To control the servos, we declare two panServo objects (responsible for camera rotation) and tiltServo (responsible for tilting). Since the servo is mechanical and does not rotate instantly, we introduce a variable for the delay required by the actuator to test the turn command (15 ms is enough)
Servo panServo, tiltServo; long interval = 15; // interval at which to control servo long previousMillis = 0; unsigned long currentMillis;
')
The previousMillis and currentMillis are used so that the control loop does not stupidly wait for the servo to complete. We check - if 15 ms has not passed since the last command, then it’s useless to command the serva - it is still busy.
The 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; }
Receive via bluetooth and execute motion commands and camera servo controls
Bluetooth module from the point of view of Arduino is just a serial (UART) port.
Therefore, we will be polling 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 - byte $ FF (the extreme positions of the servo drives and the speeds of the 255 engines are practically useless - the servos abut earlier, and the speed 250-255 does not differ, therefore this value Rarely and this will allow us to catch the beginning of the package, you can increase the reliability by complicating the algorithm, but this is enough for us).
Having found the header, we receive a byte in which the direction of the engines is encoded by 2 bits per engine. Then we read the engine speeds - 1 byte per engine (lSpeed, rSpeed) and the position of the camera's servos (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, select the directions for the right and left engines
lDirection=Direction & 0x03; rDirection=(Direction & 0x0C) >> 2;
and if the direction or speed changed from the last received command, then set the speed of the engines and rotate the camera.
Here is the whole basic 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 is practically nowhere easier :)
You can download the sketch from the project page in Google code.
We have taught the chassis to execute commands. Now we need to learn how to send them.
Who is too lazy to understand programming or reluctant to put Delphi, can download the compiled version
(works with the Logitech Extreme 3D Pro joystick or the EasyTouch Chinese gamepad).
With the rest we go further :)
We will need:
- Delphi 2010 (it is possible and Delphi 7, just a couple of lines need to be corrected in the project file)
- Component TComPort from the open ComPort Library (I have 4.11c installed)
- Components TjvHIDDevice, TjvHIDDeviceController from JEDI VCL . I am using v3.38, you can download fresher. Put it entirely, it will come in handy
Calculate the speed of the motors to set the direction
Moving forward and backward does not cause difficulties - we simply set the same speeds of the left and right motor and the same direction.
For turns in motion, we introduce the concept Steer - the deviation value from direct movement. Engine speeds are calculated for forward and backward movement 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, the deviation is subtracted, to the speed of the right engine - we add. It turns out the effect of slowing down one of the tracks and the chassis turns smoothly, without stopping completely. When moving back signs just change.
Well, we check if the speed is out of the maximum allowable values. In particular, this is useful for those who have higher voltage supply of motors than they should be - just limit the maximum speed and the motors will be intact.
Management examples:
go forward - the direction of both motors "1", the speed is the same
go back - the direction of both motors "2", the speed is the same
To rotate in moving left / right we set the same direction, the speeds are different. Turns to the side, the speed of which is less.
for turning on the spot - the speed is the same, the direction of the motors is different - it turns around the center.
stop - both motors direction "0"
Transmission of control packets via bluetooth
When adding a bluetooth module to a PC, 2 virtual COM ports are formed - one incoming, one outgoing.
To connect to the robot you just need to open the outgoing port. You can define in the list of ports in the bluetooth settings or by the search method — when connected to the correct program, the program will not swear and the LED on the module stops blinking — 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 TRCTank class that implements all actions for communicating 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, in fact, simply 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 create a header that the robot will catch (we have a byte with the 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 command sending function, the only thing worth mentioning is the packing of directions for both engines in one byte by 2 bits offset. 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 operation
Unfortunately, there is not much documentation on working with HID devices on the Internet. As a result, I went through a bunch of outdated codes that are sent to work through the midi port or consider the joysticks as a device with 2 axes and 4 buttons. I did not like this option. There was no information on the TjvJoystick component anywhere, so I came across it by chance. It's a pity, by this moment I have already written my component :) So if you are not too lazy to understand, you can use the ready-made component from the JEDI VCL.
I work with the HID device directly and analyze the Report from it by-byte. But all the joystick axes are available (EasyTouch has 4 of them) and all buttons (10-12 for 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 them to the combo box. The selected device is given to the object of the TRjoystick class by calling SelectJoystickByID (VID, PID: Word); (It is selected by VendorID and ProductID - they can be viewed, for example, in the system device manager).
The TRjoystick class performs checkout, being able to receive reports from the joystick, decrypts the values, sets the properties of the 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 pan<minPan then pan:=minPan; if tilt<minTilt then tilt:=minTilt; if pan>maxPan 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 bring the raw values of the axes coordinates to the speed range -256..256 and directions -127..128.
Since with linear control at low speeds, the motors do not have enough strength to move the robot, we introduce small dead zones (by experience) - it will only move from a certain speed value. (ApplyDeadZone (Speed, DeadX); ApplyDeadZone (Steer, DeadY);)
After taking into account the rudder, we check that the speeds did not get out of range, we 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, and also check the constraints (the servos rest mechanically, usually before they reach the digital limits of control). display the camera position on another pair of sliders, display the speeds and send the 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;
In the code, there are still various experiments, pieces responsible for preserving the selected port and joystick, saving and loading the limits of control and centering of the camera, it is possible to return control not by the joystick, but directly by tugging at 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 :)
Download the sources for BT R RC Tank on the project’s website at Google code.
I tried to remove the process of management from the first person, but shooting the screen with a camera was a thankless task, it was rather mediocre. But the general meaning is clear.
PS right now the chassis is disassembled for rework, so I can not quickly check any changes in the code on the hardware. But the version available for download is quite workable, as seen in the video.
Source: https://habr.com/ru/post/136224/
All Articles