Terasic Self-Balancing Robot страница 21 Скачать руководство пользователя | Manualshive

Страница: 21 / 46

background image

Self-Balancing Robot
User Guide

19

www.terasic.com

July 12, 2018

Function : Get Angle value (Kalman filter)

parameter :

return value :

**************************************************************************/

void Get_Angle(void)

{

int16_t ax, ay, az, gx, gy, gz;

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

Gyro_Balance=-gy;

x_angle=atan2(ax,az)*180/PI;

gy=gy/16.4;

Angle_Balance=kalman.getAngle(x_angle,-gy);

Gyro_Turn=gz;

}

First, read out the acceleration of X-, Y-, & Z- Axes and angular rate of gyroscope.

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

Because the polarity of the angular rate read out is opposite to the actual value, it is necessary to do

a negative operation.

Gyro_Balance=-gy;

As described above, the tilted angle is obtained by computing the acceleration of X-axis & Y-axis.

Use function atan2() to obtain the angle.

x_angle=atan2(ax,az)*180/PI;

As described above again, we can also get the tiled angle from Y- axis argular rate of gyroscope.

However, the read-out value needs to divide by precision.

gy=gy/16.4;

Here, 16.4 is the Sensitivity Scale Factor for the gyroscope. Why use this value are introduced in

the following descriptions.

«
...
19
20
21
22
23
...
»

Содержание Self-Balancing Robot

Страница 1: ...Self Balancing Robot User Guide 1 www terasic com July 12 2018 y94 ...

Страница 2: ...CONTENTS CHAPTER 1 USING THE SELF BALANCING ROBOT 1 1 1 CONTROL THE MOTOR 1 1 2 DETECT THE MOTOR SPEED AND DIRECTION 8 1 3 GET THE TILT ANGLE OF ROBOT 14 1 4 DETECT OBSTACLE DISTANCE 21 1 5 BALANCED SYSTEM 27 1 6 USE THE BLUETOOTH 29 1 7 USING THE IR CONTROLLER 36 ...

Страница 3: ...o describes how to control the speed of the motor As general FPGA IOs of DE10 Nano are unable to drive the motor an extra motor drive chip or circuit is needed to drive the motors the motor drive chip used on the robot is Toshiba TB6612FNG which can be used to control two DC motors simultaneously As shown in Figure 1 1 the control signals IN1 IN2 PWM control signals of motor A and B and STBY are c...

Страница 4: ...and IN2 are 0 STBY is equal to Chip Enable function The motor will stop and wait for new command when STBY logic is 0 In summary user can easily change the motor rotation direction via controlling the IN1 and IN2 logic value C Co on nt tr ro ol l R Ro ot ta at ti io on n S Sp pe ee ed d The motor speed of the motors can be controlled by controlling the Duty Cycle of the PWM signal FPGA Control Out...

Страница 5: ... IP TERASIC_DC_MOTOR_PWM v is provided in the robot demo code In this demo it is packed as Qsys component and used to control the right and left motor User can find the TERASIC_DC_MOTOR_PWM v file in the robot system CD Demonstrations BAL_CAR_Nios_Code IP TERASIC_DC_MOTOR_PWM I IP P S Sy ym mb bo ol l Figure 1 3 shows the symbol of TERASIC_DC_MOTOR_PWM v and its block diagram The main outputs are ...

Страница 6: ...ter of motor brake control User can read these registers value through Nios Table 1 2 Register Table for TERASIC_DC_MOTOR_PWM v IP Reg Address Bit Field Type Name Description Base Addr 0 31 0 R W total_dur PWM total duration value Base Addr 1 31 0 R W high_dur PWM high duration value Base Addr 2 31 3 Unused Unused bit 2 R W motor_fast_decay Motor brake control 1 for fast brake 0 for short brake 1 ...

Страница 7: ...irection always begin if motor_fast_decay begin fast decay if motor_go begin if motor_forward DC_MOTOR_IN2 DC_MOTOR_IN1 PWM 1 b1 1 b0 PWM_OUT forward else DC_MOTOR_IN2 DC_MOTOR_IN1 PWM 1 b0 1 b1 PWM_OUT reverse end else DC_MOTOR_IN2 DC_MOTOR_IN1 PWM 1 b1 1 b1 1 b0 end else begin slow decay if motor_go begin if motor_forward DC_MOTOR_IN2 DC_MOTOR_IN1 PWM 1 b1 1 b0 PWM_OUT forward else DC_MOTOR_IN2 ...

Страница 8: ...is state the motor will be clockwise rotation to drive the robot moving forward User can set motor_fast_decay as 1 and motor_go as 0 if they need a fast braking Then the below code will be executed DC_MOTOR_IN2 DC_MOTOR_IN1 PWM 1 b1 1 b1 1 b0 Finally the IN1 and IN2 logic will receive logic 0 and logic 0 respectively As shown in Table 4 1 the motor is stopped The right and left motors on the robot...

Страница 9: ...l code for controlling the rotation speed PWM reg PWM_OUT reg 31 0 total_dur reg 31 0 high_dur reg 31 0 tick always posedge clk or negedge reset_n begin if reset_n begin tick 1 end else if tick total_dur begin tick 1 end else tick tick 1 end always posedge clk begin PWM_OUT tick high_dur 1 b1 1 b0 end The tick register is the main counter total_dur represents the total_dur register described in Ta...

Страница 10: ... speed will be faster Figure 1 4 The diagram of relationship between total_dur and high_dur in PWM 1 1 2 2 D De et te ec ct t t th he e M Mo ot to or r s sp pe ee ed d a an nd d D Di ir re ec ct ti io on n Section 1 1 introduces how to control motor speed and direction this section will introduce how to use the Hall effect sensor and decoder on the motor to detect the motor speed and direction in ...

Страница 11: ...ions are different See Figure 1 7 When magnetic Rotor rotates the first sensed sensor will output wave first and the other sensor output will delay That is why the two waves have different phases So we can know the motor rotation direction according which sensor wave phase is ahead Figure 1 7 is the motor clockwise rotation status In addition we can also calculate the motor speed according to the ...

Страница 12: ... by writing code to just detect the phase and pulse number of the two signals MTRR_IN_PA MTRR_IN_PB Figure 1 8 The motor phase pins connect to FPGA E Ex xa am mp pl le e D De es sc cr ri ip pt ti io on n We do provide a Qsys IP in the Self Balancing Robot demo for users to obtain the motor speed and direction the IP can be found in folder Demonstrations BAL_CAR_Nios_Code IP motor_measure motor_mea...

Страница 13: ...system block diagram for the motor on the right R Re eg gi is st te er r T Ta ab bl le e Table 1 3 is the IP register table The Counter register in address Base Addr 0 will counts the motor pulse number The system can detect how much the motor rotates according to this register and the positive number means clockwise direction the negative number means counterclockwise direction Counter is read on...

Страница 14: ...o DO_DIRECT port and the rotation pulse will output to DO_PULSE port Then these two signal will pass to the motor_measure v IP TERAISC_AB_DECODER u_decoder DI_SYSCLK clk DI_PHASE_A phase_AB 0 DI_PHASE_B phase_AB 1 DO_PULSE conter_pulse DO_DIRECT direction Please refer to below code list in below the motor_measure IP has a 16bit Counter initial value is 16 h8000 register which is enabled only if th...

Страница 15: ..._Code software DE10_Nano_bal for the steps of system reading counter During the Self Balancing Robot system initialization process the count_en will be set to 1 after this the system will read the counter every 10ms Every time after the reading the system will set the counter back to the initial value 16 h8000 and wait for the next reading time The system will use the latest counter value to minus...

Страница 16: ...sts an angle of θ showing as Figure 1 10 Our aim is to read out this angle and feed it back to the balance system for controlling the motor rotating in the opposite direction In this way it will make the tilt of the Angle to become the ideal 0 degree as a correction Figure 1 10 The tilted angle of the balance car To obtain the tilt angle of the body the Motion Tracking device MPU 6500 on the robot...

Страница 17: ... 6500 Continue to introduce how to calculate the tilt angle of the balance car from the accelerometer Figure 1 12 ignore the horizontal acceleration of the car there is a vertical angle θ when body tilts forward or backward G is for the acceleration of gravity Resolve g vector into X and Z directions gx and gz are coordinate components for X axis and Z axis respectively θ is the tangent angle of g...

Страница 18: ... of the self balance Robot Besides it can also get the body tilted angle from Y axis angular rate by using the gyroscope in the MPU 6500 See below Figure 1 13 when the body tilts the angular rate changes as well Obtain the angle in Y axis via doing the integral calculation on the angular rate ...

Страница 19: ...er If you use the angle with larger error to balance the system it will be very difficult to stabilize the robot Therefore so a method on Angle error correction is required It is common to use Kalman Filter as a method which adopts the data fusion of two sensors gyro and accelerometer to get a more precise angle Figure 1 14 shows the comparison of the original tilt Angle blue and the Angle orange ...

Страница 20: ...res the initial operation on starting up For more details about the MPU 6500 and Register map please refer to the CD Datasheet Sensor And refer to the MPU cpp MPU h provided in CD Demonstrations BAL_CAR_Nios_Code software DE10_Nano_bal for control code E Ex xa am mp pl le e D De es sc cr ri ip pt ti io on ns s We provide the Nios II demo for balance car by using the Open core I2C module in Qsys Th...

Страница 21: ...ax ay az gx gy gz Because the polarity of the angular rate read out is opposite to the actual value it is necessary to do a negative operation Gyro_Balance gy As described above the tilted angle is obtained by computing the acceleration of X axis Y axis Use function atan2 to obtain the angle x_angle atan2 ax az 180 PI As described above again we can also get the tiled angle from Y axis argular rat...

Страница 22: ... 16 4 s The corresponding Sensitivity Scale Factor is 16 4 LSB s in the manual It is able to compute the angular rate as the same way if selecting the other ranges Figure 1 15 MPU 6500 datasheet Gyroscope Specifications Finally send the Angle value obtained from the accelerometer and gyroscope the kalman filter function to obtain the Angle of the car body with a smaller error Angle_Balance kalman ...

Страница 23: ...module is controlled mainly by TRIG and ECHO signal The detection process is described as below To start detecting the distance input High level logic signal to the TRIG I O for at least 10us The Module automatically sends eight 40 kHz and detect where there is pulse signal return The echo port will automatically output a high level logic when detecting a rebound signal The duration of the high le...

Страница 24: ...e distance high level logic time Speed of sound 340m s 2 The obstacle distance is calculated by the formula above please note that the distance unit is meter Figure 1 18 Ultrasonic Module Timing Diagram D De em mo on ns st tr ra at ti io on n D De es sc cr ri ip pt ti io on n The demo provides Qsys IP which can read the obstacle distance from the Ultrasonic module it s ...

Страница 25: ...te the duration of the high level logic signal then CPU will can read the data Figure 1 19 The IP symbol and block diagram R Re eg gi is st te er r T Ta ab bl le e Table 1 4 shows the register table of the IP measure_value register stores the ultrasonic transmission time that the module detects the obstacle distance each time Table 1 4 Register table of the IP Reg Address Bit Filed Type Name Descr...

Страница 26: ...negedge count_rst if count_rst begin measure_count 0 trig_count 0 state 0 end else begin case state 3 d0 begin sonic_trigger 1 state 1 end 3 d1 begin if trig_count 2000 begin sonic_trigger 0 state 2 end else begin trig_count trig_count 1 state 1 end end 3 d2 begin if reg_echo sonic_echo state 3 else state 2 end 3 d3 begin if reg_echo sonic_echo state 4 ...

Страница 27: ...te Diagram of the State Machine When the FPGA is running this IP will operate independently and the state machine will go to State 0 The IP include a counter to avoid the State Machine blocks in a state the counter will accumulate continually until 22 h3fffff then trigger count_rst 0 the State Machine will be reset Below is the code always posedge av_mm_clk or negedge av_mm_rst if av_mm_rst counte...

Страница 28: ...til count_rst achieves 0 then the State machine is reset State 3 Calculate the ultrasonic reflection time by using the measure_count the state machine goes to State 4 when ECHO pin signal has falling edge State 4 idle state waiting for count_rst achieve 0 reset the state machine and begin to next detection S So of ft tw wa ar re e C Co od de e After adding this IP to Qsys Nios CPU can read the ult...

Страница 29: ...roller the speed loop is controlled by PI controller and the turn loop is controlled by PD controller The balance loop is controlled by PD controller this is because the robot needs to respond quickly to the robot s angle changing and the derivative control just meets this requirement The control value of P is tilt angle of the robot the angle offset that is relative to balance status The control ...

Страница 30: ... in the int speed void function and Bias 110 and Bias 110 in the int turn float Gyro function When the Bluetooth and IR send control commands the speed loop and turn loop start working The robot does a linear motion at a set speed and rotates at a set angular speed The status of the robot needs to be sampling controlled at regular intervals In our demo the interval time is 10ms The sampling contro...

Страница 31: ...tooth and transfer the serial port protocol to FPGA for the moving control ESP32 is a powerful Bluetooth WiFi module It is easily developed The Factory code in the ESP32 module has been written with ID Number of the Self balancing robot so it is able to accept the control command from mobile phone APP via Bluetooth connection Besides it there are also many other expandable functions such as I2C Wi...

Страница 32: ...IP and then compare it with the defined instruction to get an effective instruction and control the movement of the robot In the Quartus project of the demonstration a PIO module is reserved in the Qsys system which is used for communicating the ESP32 module with FPGA I O It can be ignored as it not being used in the demo Figure 1 23 The Block Diagram of the communication between EPS32 and FPGA Th...

Страница 33: ...232 UART Core register map Offset in bytes Register Name R W Bit description 31 24 23 16 15 14 11 10 9 8 7 6 2 1 0 0 data RW 1 RAVAIL RVALID 1 PE 2 2 DATA 4 control RW 1 WSPACE 1 WI RI 1 WE RE Notes on Table 1 5 1 Reserved Read values are undefined Write zero 2 These bits may or may not exist depending on the specified DataWidth If they do not exist they read zero and writing has no effect Table 1...

Страница 34: ...ity error 15 RVALID R Indicates whether the DATA and PE fields contain valid data 23 16 RVAIL R The number of characters remaining in the read FIFO including this read The following will introduce how to use Nios to read the data transmitted by ESP32 and convert it to control commands In the main cpp path BAL_CAR_Nios_Code software DE10_Nano_bal the codes for Bluetooth controlling as below Bluetoo...

Страница 35: ...false Car Set_TurnFORWARD break case CMD_BACKWARD Backward led3 0x02 demo false Car Set_TurnBACKWARD break case CMD_LEFT Left led3 0x04 demo false Car Set_TurnLEFT break case CMD_RIGHT Right led3 0x08 demo false Car Set_TurnRIGHT break case CMD_STOP Stop led3 0x00 demo false Car Pause break First read the receiving data register Receiving data register offset address is 0 ...

Страница 36: ...0 that is the data is valid Take the last 8 bits into the array and then receive the next 8 bits cyclically when the received data is 0x0a which is defined as end of transmission character When transferring a control command completed on the mobile app it will send out this value for data sending finished Next to the command comparision all commands from the bluetooth are definded in the command h...

Страница 37: ...e to parse the command The source codes in main cpp are as below Function Bluetooth Command Parsing parameter Command Command ID return value Command Parsing data bool CommandParsing char pCommand int pCommandID int pParam bool bFind false int nNum i j x 0 bool find_equal false char Data 10 0 nNum sizeof gCommandList sizeof gCommandList 0 for i 0 i nNum bFind i if strncmp pCommand gCommandList i s...

Страница 38: ...hone app to control the robot via the Bluetooth using an infrared remote control is also a simple and convenient option The infrared remote controller included with the kit package is an infrared remote controller that uses the NEC protocol It uses 38K HZ frequency modulation to emit control signals to the infrared receiver on the robot The FPGA decoder IP will decode the signal to control the rob...

Страница 39: ... logic 0 Figure 1 25 NEC protocol leader length variable block The IR receiver on the balanced car can decode the carrier frequency of 38 kHz and reverse the received signal Therefore it should be noted that the signal processed in the FPGA will be opposite to that of the transmitter as shown in Figure 1 26 Figure 1 26 The signal received by IR receiver S Se el lf f B Ba al la an nc ci in ng g R R...

Страница 40: ...igure 1 27 IR controller key function Table 1 7 Key code information for each Key on remote controller IR Controller key Custom Code Key code Inversed Key Code D 3 0 D 7 4 D 11 8 D 15 12 D 19 16 D 23 20 D 27 24 D 31 28 68 B6 F0 0F 68 B6 31 CE 68 B6 01 FE 68 B6 00 FF ...

Страница 41: ...ncing Robot User Guide 39 www terasic com July 12 2018 68 B6 10 EF 68 B6 20 DF 68 B6 30 CF 68 B6 40 BF 68 B6 50 AF 68 B6 60 9F 68 B6 70 8F 68 B6 80 7F 68 B6 90 6F 68 B6 21 DE 68 B6 A1 5E 68 B6 E1 1E 68 B6 B1 4E ...

Страница 42: ... diagram of FPGA using TERASIC_IRM v to decode IR control signal The IR receiver receives signal and send the signal to this IP this IP provides Avalon interface a submodule irda_receive_terasic v will do the decoding work The decoded Custom Code and Key Code information will be sent to TERASIC_IRM V and stored in the register At the same time an interrupt signal is sent to the CPU and the Nios CP...

Страница 43: ...e Nios CPU receives IR interrupt signal the Nios CPU will read the register value and compare it with the defined code table and then judge the meaning of the instruction IR controller key codes 32bits are defined in the file IrRx h as followed code lines typedef enum IR_POWER 0xed126b86 IR_CH_UP 0xe51a6b86 IR_CH_DOWN 0xe11e6b86 IR_VOL_UP 0xe41b6b86 IR_VOL_DOWN 0xe01f6b86 IR_MUTE 0xf30c6b86 IR_ADJ...

Страница 44: ...le there are code lines for detecting IR IR Remote control if IR IsEmpty Command_IR IR Pop Command_IR IORD IR_RX_BASE 0x00 printf 04xh r n Command_IR switch Command_IR case CIrRx IR_NUM_5 Stop led3 0x00 demo false Car Pause break case CIrRx IR_NUM_2 Forward if mode 0x02 if distance 15 0 led3 0x01 flag 0x02 demo false Car Set_TurnFORWARD else led3 0x01 flag 0x00 demo false Car Set_TurnFORWARD break...

Страница 45: ...18 Using IR IsEmpty to detect whether IR data is received or not using IR Pop to read register DATA_BUF value and compare it with the defined code table and then judge the meaning of the instruction and control the Self Balancing Robot moving forward or backward ...

Страница 46: ...tact information where you can get help if you encounter problems Terasic Inc 9F No 176 Sec 2 Gongdao 5th Rd East Dist Hsinchu City Taiwan 300 70 Email support terasic com Web www terasic com R Re ev vi is si io on n H Hi is st to or ry y Date Version Changes 2018 03 16 First publication 2018 07 11 V1 1 Modify Figure 1 21 and Figure 1 22 ...

Отзывы:

Нет отзывов

Похожие инструкции для Self-Balancing Robot

Бренд: WowWee Страницы: 20

Бренд: I Love robots Страницы: 12

Dobot Vision Kit

Бренд: ShenZhen Yuejiang Technology Страницы: 19

Бренд: TEMI Страницы: 28

Бренд: YASKAWA Страницы: 70

Бренд: Variobot Страницы: 97

Бренд: Ebotics Страницы: 78

Бренд: Velleman Страницы: 30

Бренд: Epson Страницы: 84

Бренд: Epson Страницы: 274

Бренд: Epson Страницы: 112

ProSix S5 Series

Бренд: Epson Страницы: 211

Бренд: Epson Страницы: 122

SCARA GX Series

Бренд: Epson Страницы: 236

Бренд: Epson Страницы: 184

Part Feeding 7.0 IF-80

Бренд: Epson Страницы: 58

Бренд: Epson Страницы: 64

Бренд: Parallax Страницы: 24

Бренды по названию

0 1 2 3 4 5 6 7 8 9 A B C D E F G H I J K L M N O P Q R S T U V W X Y Z

Популярные бренды

Загрузить еще бренды