Download
Share
Print this page
  1. Manuals
  2. Brands
  3. Terasic Manuals
  4. Robotics
  5. Self-Balancing Robot
  6. User manual

Terasic Self-Balancing Robot User Manual

Hide thumbs Also See for Self-Balancing Robot:

Advertisement

Quick Links

Download this manual
y94
Self-Balancing Robot
www.terasic.com
1
User Guide
July 12, 2018

Advertisement

loading
Need help?

Need help?

Do you have a question about the Self-Balancing Robot and is the answer not in the manual?

Questions and answers

Subscribe to Our Youtube Channel

Related Manuals for Terasic Self-Balancing Robot

Summary of Contents for Terasic Self-Balancing Robot

  • Page 1 Self-Balancing Robot www.terasic.com User Guide July 12, 2018...

  • Page 2: Table Of Contents

    CONTENTS USING THE SELF-BALANCING ROBOT CHAPTER 1 ..................1 1.1 C .............................. 1 ONTROL THE OTOR 1.2 D ......................8 ETECT THE OTOR SPEED AND IRECTION 1.3 G ..........................14 ET THE TILT ANGLE OF OBOT 1.4 D ..........................21...

  • Page 3: Using The Self-Balancing Robot

    Chapter 1 Using the Self-Balancing Robot To keep the robot in vertical balance status, user need to control the rotation of the motor and make sure the accelerated rotation direction is reversed to the robot tilt direction. User need to learn how to control the rotation direction and speed of the motor.
  • Page 4 In summary, user can easily change the motor rotation direction via controlling the IN1 and IN2 logic value. ◼ ◼ The motor speed of the motors can be controlled by controlling the Duty Cycle of the PWM signal. Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 5 The DC_MOTOR_IN1 and DC_MOTOR_IN2 are the control signals that can control the motor rotation direction and stopping, which has been described in previous section. PWM control signal is responsible for controlling the motor speed. Self-Balancing Robot www.terasic.com User Guide...
  • Page 6 31:3 Unused Unused bit Motor brake control motor_fast_decay 1 for fast brake Base Addr + 0 for short brake Motor direction control : motor_forward 1 for forward 0 for backward Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 7 {DC_MOTOR_IN2, DC_MOTOR_IN1,PWM} <= {1'b1, 1'b1,1'b0}; 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, DC_MOTOR_IN1,PWM} <= {1'b0, 1'b1,PWM_OUT}; // reverse else Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 8 ( DE10_Nano_Bal.v) are defined opposite, as described in code below: Qsys u0 ( //clock && reset .clk_clk (FPGA_CLK2_50), // clk.clk） .reset_reset_n (1'b1), // reset.reset_n //right motor control .dc_motor_right_conduit_end_1_pwm (MTR_PWMA), dc_motor_right_conduit_end_1.pwm .dc_motor_right_conduit_end_1_motor_in1 (MTRR_P), .motor_in1 .dc_motor_right_conduit_end_1_motor_in2 (MTRR_N), .motor_in2 Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 9 The tick register is the main counter, total_dur represents the total_dur register described in Table 4-2. When the tick value equals to total_dur setting value, the whole counter will be reset and Self-Balancing Robot www.terasic.com User Guide July 12, 2018...

  • Page 10: Detect The Motor Speed And Direction

    Hall effect sensors, and then, the Hall effect sensor magnetic force will change and generate Hall effect voltage, a digital circuit will process the Hall effect voltage and output square wave pulse (See Figure 1- Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 11 In addition, we can also calculate the motor speed according to the pulse number. Over a period of time, the faster the motor rotates, the more pulses it generates. Self-Balancing Robot www.terasic.com User Guide...
  • Page 12 Figure 1- 8 The motor phase pins connect to FPGA ◼ ◼ 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_measure.v ⚫...
  • Page 13 Counters. When count_en is set to 1, the Counter register will start to count. Table 1- 3 motor_measure.v IP Register Reg Address Bit Filed Type Name Description Base Addr + 0 31:16 Unuse Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 14 If the motor rotates counterclockwise, the Counter register also will decrement with the number of pulses returned by the motor. The system will periodically read the value of this Counter register to acquire the current Self-Balancing Robot www.terasic.com User Guide...
  • Page 15 \Demonstrations\BAL_CAR_Nios_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.

  • Page 16: Get The Tilt Angle Of Robot

    This section describes how to obtain the tilt angle of the Self-Balancing Robot, and how to get an angularity correction to keep the robot balance. ◼ ◼ The idealized state of the balance car is to maintain a vertical 90-degree angle to the ground.
  • Page 17 X-axis and Z-axis respectively, θ is the tangent angle of gx or gz. Read out the values of gx & gz from the accelerometer in the MPU-6500. Get the degree of angle θ by using the function θ = arctan(gx/gz). Self-Balancing Robot www.terasic.com User Guide...
  • Page 18 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. Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 19 Kalman Filter. From the figure, you can see that the change in angle is large different for unfiltered angle (blue). By working with these data, the balance of the car body could not be stable. However, the varying amplitude for the orange Angle after filtering is significantly reduced. Self-Balancing Robot www.terasic.com User Guide...
  • Page 20 We provide the Nios II demo for balance car by using the Open-core I2C module in Qsys. This module allows Nios II to access the mpu-6500 through the I2C interface. function obtaining tilted angle provided path: \Demonstrations\BAL_CAR_Nios_Code\software\DE10_Nano_bal\main.cpp, the main codes are as following: /************************************************************************** Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 21 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. Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 22 When dealing with the turning of the body, the system needs to refer to the angular speed of the z axis. Gyro_Turn=gz; The parameters above will be provided to system for balance PID(Proportional–Integral–Derivative) controlling to feed back the actual condition of the body to achieve a balanced state. Self-Balancing Robot www.terasic.com User Guide July 12, 2018...

  • Page 23: Detect Obstacle Distance

    Figure 1- 16 Ultrasonic module working diagram Self-Balancing Robot www.terasic.com User Guide...
  • Page 24 The obstacle distance is calculated by the formula above, please note that the distance unit is meter. Figure 1- 18 Ultrasonic Module Timing Diagram ◼ ◼ The demo provides Qsys IP which can read the obstacle distance from the Ultrasonic module, it’s Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 25 Table 1- 4 Register table of the IP Reg Address Bit Filed Type Name Description 31:22 Unuse Base Addr + 0 Sonic wave propagation 21:0 measure_value time ⚫ ⚫ Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 26 @(posedge av_mm_clk or negedge count_rst) if(~count_rst) begin measure_count<=0; trig_count<=0; state<=0; else begin case(state) 3'd0:begin sonic_trigger<=1; state<=1; 3'd1:begin if(trig_count==2000) begin sonic_trigger<=0; state<=2; else begin trig_count<=trig_count+1; state<=1; 3'd2:begin if(!reg_echo&sonic_echo) state<=3; else state<=2; 3'd3:begin if(reg_echo&!sonic_echo) state<=4; Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 27 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) counter<=0; else if(counter==22'h3fffff) counter<=0; Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 28 Note: As described in previous, the system clock in the IP is 50MHz and the distance unit is centimeter, apply to the formula, it is data * 340 * 100 / (2 * 50 * 1000000) = data*34.0/100000. Self-Balancing Robot www.terasic.com...

  • Page 29: Balanced System

    This section will introduce balancing system control of the self-balancing robot. This section describes how a robot can maintain its uprightness, speed, and state of turn and how it is controlled. As described in the previous sections, the tilt-angle measurement and rotation-angle measurement of the robot are implemented by using the MPU-6500 to measure acceleration and gyroscope.
  • Page 30 Bluetooth/IR control signal, ultrasonic detection obstacle distance (for obstacle avoidance) signal and power monitoring signal. The main routine flow chart is shown in Figure 1- Self-Balancing Robot www.terasic.com User Guide July 12, 2018...

  • Page 31: Use The Bluetooth

    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, WiFi and SPI data transmission, etc.
  • Page 32 ◼ Figure 1- 23 is for the system architecture of Self-balancing robot using the ESP32. When the ESP32 receives the string command from the app via the Bluetooth protocol, it will be transferred to the UART interface and pass to the UART IP of the QSYS within the FPGA. In this way, the NIOS CPU can easily read the value of the data register in the IP, and then compare it with the defined instruction to get an effective instruction and control the movement of the robot.
  • Page 33 Data Register format, the bit 8~0 are for the data transferring, bit 23~16 are for indicating the number of characters remaining in the read FIFO. We can know the data transmission completed or not via bit 23~16. Self-Balancing Robot www.terasic.com User Guide...
  • Page 34 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: // Bluetooth control temp=IORD(UART_BT_BASE,0x00); number=temp>>16; if(number!=0) szData[i]=temp&0xff; i++; if((temp&0xff)==0x0a) i=0; if(CommandParsing(szData, &Command_EPS32, &Param)){ switch(Command_EPS32){ case CMD_FOWARD: //Forward Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 35 //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) Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 36 * Command.h #ifndef COMMAND_H_ #define COMMAND_H_ #include "terasic_includes.h" typedef enum{ CMD_FOWARD=1, CMD_BACKWARD, CMD_LEFT, CMD_RIGHT, CMD_STOP, CMD_AKBT, CMD_ATDM, CMD_ATUTON, CMD_ATUTOFF, }COMMAND_ID; typedef struct{ char szCommand[10]; int CommandId; bool bParameter; Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 37 , x=0; bool find_equal = false; char Data[10]={0}; nNum = sizeof(gCommandList)/sizeof(gCommandList[0]); for(i=0;i<nNum && !bFind;i++){ (strncmp(pCommand, gCommandList[i].szCommand, strlen(gCommandList[i].szCommand)) == 0){ *pCommandID = gCommandList[i].CommandId; (gCommandList[i].bParameter){ //*pParam = 10; //?? //for(j=0;pCommand[j]!=0x0a;j++){ for(j=0;pCommand[j]!=0x0d;j++){ if(find_equal==true){ Data[x] = pCommand[j]; Self-Balancing Robot www.terasic.com User Guide July 12, 2018...

  • Page 38: Using The Ir Controller

    ◼ The NEC format composes of a Leader Code, a 16-bit Custom Code, and a 8-bit Key Code. The Leader Code is transmitted first, which contains 9ms carrier frequency and 4.5ms zero signal, then Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 39 Figure 1- 26 The signal received by IR receiver ◼ ◼ for IR controller’s function and key and its corresponding Inversed Key Code, Figure 1- 27 Key Code and Custom Code are shown in Table 1-7. Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 40 Figure 1- 27 IR controller key function Table 1- 7 Key code information for each Key on remote controller Inversed Key Custom Code Key code Code IR Controller key D[19:16] D[3:0] D[7:4] D[11:8] D[15:12] D[27:24] D[31:28] D[23:20] Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 41 Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 42 ◼ ◼ We provide an IR control decoder IP in the Self-Balancing Robot demonstrations folder, which location is \Demonstrations\BAL_CAR_Nios_Code\IP\TERASIC_IRM\TERASIC_IRM.v. Figure 1- 28 is the block 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.
  • Page 43 0xe11e6b86, IR_VOL_UP 0xe41b6b86, IR_VOL_DOWN 0xe01f6b86, IR_MUTE 0xf30c6b86, IR_ADJ_LEFT 0xeb146b86, IR_ADJ_RIGHT 0xe7186b86, IR_PLAY_PAUSE = 0xe9166b86, IR_NUM_0 0xff006b86, IR_NUM_1 0xfe016b86, IR_NUM_2 0xfd026b86, IR_NUM_3 0xfc036b86, IR_NUM_4 0xfb046b86, IR_NUM_5 0xfa056b86, IR_NUM_6 0xf9066b86, IR_NUM_7 0xf8076b86, IR_NUM_8 0xf7086b86, Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 44 (!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; Self-Balancing Robot www.terasic.com User Guide July 12, 2018...
  • Page 45 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. Self-Balancing Robot www.terasic.com...
  • Page 46 Additional Information Here is the contact 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 Date Version Changes 2018.03.16...