02 SmartRobotCarV4.0_Move - TB6612
02 SmartRobotCarV4.0_Move - TB6612
Introduction:
In this lesson, we will teach you how to make the car move as you wish
by controlling the motor.
Material Preparation:
First of all, let’s take a look at the 4 motors we used in our kit.
DC motor is the inductive load of large current (load with inductance
parameters), while the IO port of Arduino UNO single-chip microcomputer
we used has weak load capacity (output current capacity), and the driving
capacity is not enough to supply enough current for the motor to rotate.
Therefore, we chose to use the motor driver chip TB6612. (Although there
may be slight differences in the performance of different driver chips, they
are generally the same in usage.):
Please open the folder of last level for details: Related chip information ->
TB6612 and SmartCar-Shield-V1.1
Moreover, the pins of TB6612 connected to UNO can also be seen in this
diagram.
EN stands for enable pin, which changes the motor speed by changing
PWM (0~255).
PH stands for phase pin, and the rotation direction (positive/reverse) of the
motor is changed by changing the pin level (0/1).
In motor A, PWMA is connected to the UNO~D5 pins and AIN1 is connected
to the UNO D7 pin.
In motor B, PWMB is connected to the UNO~D6 pins and BIN1 is connected
to the UNO D8 pin.
Finally, the driving method can be obtained from Related chip i nformation
->TB6612.
Then, we can start writing program according to the flow chart below.
Please open Demo1 in the current folder:
Motor AIN1:1
Init PWMA: speed
Right BIN1: 0
PWMB: speed
AIN1:0
Right PWMA: speed
Backward BIN1: 0
PWMB:
speed/2
Stop AIN1:1
PWMA:speed/2
BIN1:1
PWMB: speed
AIN1:0
PWMA:speed/2
BIN1:0
PWMB: speed
AIN1:1
PWMA: 0
BIN1:1
PWMB: 0
// in DeviceDriverSet_xxx0.h
/*Motor*/
class DeviceDriverSet_Motor
{
public:
void DeviceDriverSet_Motor_Init(void);
#if _Test_DeviceDriverSet
void DeviceDriverSet_Motor_Test(void);
#endif
void DeviceDriverSet_Motor_control(boolean direction_A, uint8_t speed_A,
boolean direction_B, uint8_t speed_B,
boolean controlED
);
private:
#define PIN_Motor_PWMA 5
#define PIN_Motor_PWMB 6
#define PIN_Motor_BIN_1 8
#define PIN_Motor_AIN_1 7
#define PIN_Motor_STBY 3
public:
#define speed_Max 255
#define direction_just true
#define direction_back false
#define direction_void 3
// in DeviceDriverSet_xxx0.h
// in DeviceDriverSet_xxx0.h
// in DeviceDriverSet_xxx0.h
//in DeviceDriverSet_xxx0.cpp
DeviceDriverSet_Motor AppMotor;
void setup()
{
AppMotor.DeviceDriverSet_Motor_Init();
}
After initializing the pins, we can change the status of the pins to control
the motor and make the motor rotate as we want.
1. Car Motion
//in ApplicationFunctionSet_xxx0.cpp
static void ApplicationFunctionSet_SmartRobotCarMotionControl
(SmartRobotCarMotionControl direction, uint8_t is_speed)
parameter:
direction: Passing motion direction control sequence.
is_speed: 0~255
//in ApplicationFunctionSet_xxx0.cpp
//in DeviceDriverSet_xxx0.cpp
void DeviceDriverSet_Motor::DeviceDriverSet_Motor_control
(boolean direction_A, uint8_t speed_A, //motor A
boolean direction_B, uint8_t speed_B, //motor B
boolean controlED //AB enable true
)
parameter:
direction_A,direction_B: Passing motor rotation direction flag.
speed_A,speed_B: range 0~255
// in DeviceDriverSet_xxx0.h
In the end, make the car move in each direction for one second after
2s waiting.
void setup() {
AppMotor.DeviceDriverSet_Motor_Init();
delay(2000);
for (Application_SmartRobotCarxxx0.Motion_Control = 0;
Application_SmartRobotCarxxx0.Motion_Control < 9;
Application_SmartRobotCarxxx0.Motion_Control =
Application_SmartRobotCarxxx0.Motion_Control + 1)
{
ApplicationFunctionSet_SmartRobotCarMotionControl
(Application_SmartRobotCarxxx0.Motion_Control /*direction*/,
255 /*speed*/);
delay(1000);
}
}
Upload the program. (Pleasetoggle the“Upload-Cam”button to“Upload”
when uploading the program.) Place the car on the ground and turn on the
switch after the program has been uploaded successfully. Then, you will see
the car move forward, move backward, turn left, turn right, turn front-left,
left-rear, turn front-right, turn right-rear at 1s intervals and finally stop.
After Demo1 was executed, if you are careful enough, you may notice that
there are some deviations in the direction of the Smart Robot Car when it
moves forward and backward. On the one hand, different terrain has
different friction, on the other hand, even the same type of motor may have
slight differences in hardware characteristics and slowdown structure, as
well as disturbances during operation, such as momentary wheel slippage,
tiny obstacles and other factors that may cause the left and right wheels to
rotate at different speeds when going "straight". That’s why it deviates from
the path slightly.
You can upload the program of Demo2 to keep observing if you don't see it
clearly in the Demo1.
//in MPU6050_getdata.h
class MPU6050_getdata
{
public:
bool MPU6050_dveInit(void);
bool MPU6050_calibration(void);
bool MPU6050_dveGetEulerAngles(float *Yaw);
public:
//int16_t ax, ay, az, gx, gy, gz;
int16_t gz;
//float pith, roll, yaw;
unsigned long now, lastTime = 0;
float dt;
float agz = 0;
long gzo = 0;
};
//in MPU6050_getdata.cpp
bool MPU6050_getdata::MPU6050_dveInit(void)
{
Wire.begin();
uint8_t chip_id = 0x00;
uint8_t cout;
do
{
chip_id = accelgyro.getDeviceID();
Serial.print("MPU6050_chip_id: ");
Serial.println(chip_id);
delay(10);
cout += 1;
if (cout > 10)
{
return true;
}
} while (chip_id == 0X00 || chip_id == 0XFF);
accelgyro.initialize();
return false;
}
Because of the data drift problem of MPU6050, we had better use mean
filtering when sampling, and then obtain the z-axis angular velocity value
when the car is first placed.
//in MPU6050_getdata.cpp
bool MPU6050_getdata::MPU6050_calibration(void)
{
unsigned short times = 100;
for (int i = 0; i < times; i++)
{
gz = accelgyro.getRotationZ();
gzo += gz;
}
gzo /= times;
// gzo = accelgyro.getRotationZ();
return false;
}
void setup() {
Serial.begin(9600);
AppMPU6050getdata.MPU6050_dveInit();
delay(2000);
AppMPU6050getdata.MPU6050_calibration();
}
From MPU6050, the sensitivity factor is known to be 131 LSB (count) /°/s.
After obtaining the original sensor data, we can calculate the angular
velocity by dividing the original sensor data by their sensitivity scaling factor:
angular velocity along the Z-axis = (gyro z-axis raw data /131) °/s.
Because the execution time of the program is less than 1S, we need to
multiply the final result by time.
Finally, the yaw value can be obtained by integrating the resulting angular
velocity.
//in MPU6050_getdata.cpp
KP scale parameter is the best data we can get through repeated debugging.
//in ApplicationFunctionSet_xxx0.cpp
The yaw data is acquired at the first startup, and the data is updated
every ten millimeters.
//in ApplicationFunctionSet_xxx0.cpp
AppMPU6050getdata.MPU6050_dveGetEulerAngles(&Yaw);
is_time = millis();
}
......
}
Then, we need to perform P proportional control in the PID algorithm
on the acquired input data.
//in ApplicationFunctionSet_xxx0.cpp
//in ApplicationFunctionSet_xxx0.cpp
http://www.elegoo.com