Slide 1

Hardware project examples (v.5c)1 CENG4480 Digital Systems Design Hardware project examples Slide 2 Hardware project examples (v.5c)2 Overview 1)Self balancing robot 2)Music robot 3)Vision robot 4)FPGA projects Slide 3 Project 1: Self balance vehicle / robot Segway http://www.segway.com/ Hardware project examples (v.5c)3 http://www.ebay.com/itm/Toy-Mechanical-Robot-Remote-Walking-Dual-Wheel-Balancing-Multiple-Modes-Kids-Toy-/111728506257 http://news1.ghananation.com/latest-news/300907-it-s-the-segway-section-chinese-cops-on-patrol-for-shoplifters-on-electric-scooters.html Slide 4 Khw1404 FYP a self balanced robot by HA Ngo Lam, LEE Yiu Hei supervised by Prof. KH Wong Hardware project examples (v.5c)4 Slide 5 The basic idea Motion against the tilt angle, so it can stand upright Hardware project examples (v.5c)5 Slide 6 Exercise 5.1 IMU board Inertial Measurement Unit board l Direction (electronic compass) What does it measure? _____________. Absolute or relative? _____ l Position (accelerometer) What does it measure?_____________. Absolute or relative? _____ l Angular velocity (gyroscope) What does it measure?_____________. Absolute or relative? _____ l Pressure (Barometer) What does it measure?_____________. Absolute or relative? _____ l Temperature What does it measure?______________. Absolute or relative? _____ Hardware project examples (v.5c) 6 Student ID: __________________ Name: ______________________ Date:_______________ (Submit this at the end of the lecture.) Slide 7 The algorithm is based on two Filter methods Kalman Filter Complementary Filter Hardware project examples (v.5c)7 Slide 8 Algorithm - Kalman Filter New Measurement Hardware project examples (v.5c)8 Data Knowledge Base Output Prediction of New Data Update New Data Using Weight Output Average New Measurement Slide 9 See how nosier can be removed by Kalman filter Hardware project examples (v.5c)9 Slide 10 Kalman code (lab5: IMU) // Kalman filter module float Q_angle = 0.001; float Q_gyro = 0.003; float R_angle = 0.03; float x_angle = 0; float x_bias = 0; float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; float dt, y, S; float K_0, K_1; float kalmanCalculate(float newAngle, float newRate,int looptime) { dt = float(looptime)/1000; x_angle += dt * (newRate - x_bias); P_00 += - dt * (P_10 + P_01) + Q_angle * dt; P_01 += - dt * P_11; P_10 += - dt * P_11; P_11 += + Q_gyro * dt; y = newAngle - x_angle; S = P_00 + R_angle; K_0 = P_00 / S; K_1 = P_10 / S; x_angle += K_0 * y; x_bias += K_1 * y; P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; return x_angle;} Hardware project examples (v.5c) 10 Slide 11 Complementary filter to combine gyroscope and accelerometer Hardware project examples (v.5c)11 Gyroscope Accelerometer Integration Kalman Filter Angle Slide 12 Complementary Filter to combine gyroscope and accelerometer Combine two sensors for good measurement: Gyroscope l response faster but has drift over time Accelerometer l response slower but no drift l Positive If not moving, accelerometer will give accurate reading of tilt angle l Negative Accelerometers are slower to respond than Gyro's Accelerometers are prone to vibration/noise Hardware project examples (v.5c)12 Ref: http://www.hobbytronics.co.uk/accelerometer-gyrohttp://www.hobbytronics.co.uk/accelerometer-gyro Demo https://www.youtube.com/watch?v=jUxU7jsCa-U Slide 13 Complementary Filter code in CENG4480: lab 6 Read_acc(); Read_gyro(); //compute interval since last sampling time in millisecond interval = newMilli - lastMilli; lastMilli = newMilli; // save for next loop, please note interval // will be invalid in first sample but we don't use it Ayz=atan2(RwAcc[1],RwAcc[2])*180/PI; //angle measured by accelerometer Ayz-=offset; //adjust to correct balance point Angy = 0.98*(Angy+GyroIN[0]*interval/1000)+0.02*Ayz; //complement filter kang = kalmanCalculate(Angy, GyroIN[0],interval); //kalman filter Serial.print(kang); LRspeed = 0; Hardware project examples (v.5c) 13 Slide 14 Exercise 5.2 Explain the operation of the complementary filter and why do we need to have this filter. Answer: Hardware project examples (v.5c)14 Slide 15 A short introduction to digital filter Analog filter l Using capacitor resistors, inductors with op-amp Digital filter (using digital hardware or software) l FIR ( Finite Impulse Response, non-iterative) l IIR (Infinite impulse response, iterative) Hardware project examples (v.5c)15 Gain G(f) in dB 3dB Frequency Slide 16 Example: Finite Impulse Response (FIR) Low pass filter (LPF) (one pole) Algorithm For an input of x(n): l x(time =0), x(time =1),x(time=2), l Or X(n=0), x(n=1), x(n=2) y(n)=g*x(n)+a*x(n-1), if g=0.5,a=0.5 y(n)=0.5(x(n)+x(n-1)) //=averaging filter, Only use previous signal samples Simple to implement (non-iterative), and stable Hardware project examples (v.5c)16 http://soundlab.cs.princeton.edu/learning/tutorials/DSP/DSP.h tml Analog LPF Equivalent digital (FIR) LPF Slide 17 Exercise 5.3 a)If you have a signal x(0),x(1),x(2),x(3),x(4),x(5). Write the filter output y(n) for time is n=1,2,3,4,5 if the filter (with g=0.6, a=0.4) is y(n)=g*x(n)+a*x(n-1) b)If x(0)=4,x(1)=3,x(2)=7,x(3)=5,x(4)=6.2,x(5)=9, what is the value of y(5)? Answers: Hardware project examples (v.5c)17 Slide 18 Algorithm - PID Control to stabilize robot Hardware project examples (v.5c)18 Setpoint Proportional control Kp * proportional term Integral control Ki * integral term Derivative control Kd * derivative term Sum Motor Speed IMU Angle Slide 19 TUMBLER Version1 Hardware project examples (v.5c)19 Slide 20 TUMBLER 3 Hardware project examples (v.5c)20 Slide 21 TUMBLER Version3 in action Hardware project examples (v.5c)21 Slide 22 Design Hardware project examples (v.5c)22 Slide 23 Kalman code (lab5: IMU) // Kalman filter module float Q_angle = 0.001; float Q_gyro = 0.003; float R_angle = 0.03; float x_angle = 0; float x_bias = 0; float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; float dt, y, S; float K_0, K_1; float kalmanCalculate(float newAngle, float newRate,int looptime) { dt = float(looptime)/1000; x_angle += dt * (newRate - x_bias); P_00 += - dt * (P_10 + P_01) + Q_angle * dt; P_01 += - dt * P_11; P_10 += - dt * P_11; P_11 += + Q_gyro * dt; y = newAngle - x_angle; S = P_00 + R_angle; K_0 = P_00 / S; K_1 = P_10 / S; x_angle += K_0 * y; x_bias += K_1 * y; P_00 -= K_0 * P_00; P_01 -= K_0 * P_01; P_10 -= K_1 * P_00; P_11 -= K_1 * P_01; return x_angle;} Hardware project examples (v.5c) 23 Slide 24 PID code in CENG4480:lab6 if ((abs(kang)>=minangle)&&(abs(kang)