CONSTRUCTION GUIDE The pendulumrobot. Robobox. Level X

Transcription

1 CONSTRUCTION GUIDE The pendulumrobot Robobox Level X

2 This box allows us to introduce a key element of modern robotics, the gyroscope accelerometer sensor. As its name suggests, this component has two parts, one calculating the angle at which our robot is (the gyroscope) and the other the accelerations and forces felt (the accelerometer). In cell phones or drones, these elements have become indispensable, we will explain here the operation. 10X male female Peaces 1x Gyroscope 1x link UNO Instructions We suggest that you follow these instructions step by step. Additional details are available on your member space on Robobox.io. Please don t hesitate to ask any question, we will answer them promptly. Good luck!

3 _0_ INTR0DUCTI0N The goal of our program is to stabilize our robot with a gyroscope. The gyroscope sends series of signals that indicate the position and movements of our robot. Unfortunately there can sometimes be "noise" in these signals, that is, inconsistencies. The received data may also degrade over time, so we will needto"smooth"thisdatawithafilter. We will finally have to adapt the power of our engine to the necessary grinding to straighten the robot. This is done through an algorithm called "PID" which we will unveil the operation. We measure the position of the robot Improved measurement of the gyroscope thanks to a filter 1 Every 10ms The difference between the current position and the ideal position is calculated2 We compare the position of the robot with respect to its equilibrium position Motors are operated in the required direction and with the correct power 3 It tells the engine to go in a certain direction (forward or backward) and at a certain power thanks to a PID algorithm

4 _1_ PROGRAMATION Programming the pendulum robot is quite demanding, so we advise you to follow step by step the steps illustrated below. Step 1 #include"i2cdev.h" #include"mpu6050.h" #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include"wire.h" #endif #define OUTPUT_READABLE_ACCELGYRO At first we will code some instructions in the pre compiler, we have already encountered this language in the past when we introduced booksellers. The instructions for the pre compiler are preceded by a hashtag # and are all executed before the rest of the code(regardless of their position). Here, we will first introduce several libraries: I2Cdev will help us communicate through the I2C protocol. The I2C communication allows the exchange of data between two electronic devices through a two-wire connection: one transmits thetimeandtheotherthedata. Thus once the synchronization is effective both devices know what type of data is transmitted through the device. Step 2 Step 3 MPU6050 accelgyro; int16_t ax, ay, az; int16_t gx, gy, gz; floati = 0; float prev_input = 0; intpin1moteur1 = 12; intpin2moteur1 = 8; int pinpmoteur1 = 11; intpin1moteur2 = 2; int pin2moteur2 = 4; int pinpmoteur2 = 5; floatpitch = 0; long t; long t_next; long t_real; We will now create an object "MPU6050" corresponding to the gyroscope. We will then initialize 6 variables, 3 corresponding to the accelerometer (ax, ay, az) and 3 corresponding to the gyroscope (gx, gy, gz). The variable 'prev_input' is also added to this step, we will explain its usefulness later. In step 3, we finish initializing values by defining the motor variables and the pitch, which will correspond to the inclination of our robot. We also add variables t, t_next and t_real which willhelpusmanagethetimeinourprogram.

5 _1_ PROGRAMATION Step 4 voidsetup() { Wire.begin(); Serial.begin(38400); Serial.println(" Initializing the I2C com..."); accelgyro.initialize(); Serial.println(" Connection test..."); Serial.println(accelgyro.testConnection()? "Connection MPU6050 OK" : "Connection MPU6050 Failed"); pinmode(pin1motor1,output); pinmode(pin2motor1,output); pinmode(pinpmotor1,output); pinmode(pin1motor2,output); pinmode(pin2motor2,output); pinmode(pinpmotor2,output); The setup () function will essentially serve us to initialize the communication with the gyroscope and to configure our L293D component (essential to the movement of our robot). At first we start the communication with the gyroscope thanks to the '.begin ()' method of the 'wire' object. The 6th line of our setup () functionhasashapethatwe have not seen enough: objet.methode()? Text1 : Text2 This corresponds to a succinct form of: If(object.methode() == True){ Texte 1 { Texte 2 Wethendefinetheconnectionsofthecaraswehavedonesincemonth4.

6 _1_ PROGRAMATION Step 5 voidloop() { t = millis(); t_next = t+10; accelgyro.getmotion6(&ax, &ay, &az, &gx, &gy, &gz); pitch = filteredpitch(az,ax,gy,pitch); int pid_res = (int)pid(pitch); //Time Adj t_real= millis(); delay(t_next- t_real); int vit_mot = abs(pid_res); if (pid_res> 0){ dirmoteur(1,1,vit_mot); dirmoteur(2,1,vit_mot); ; if (pid_res<0){ dirmoteur(1,-1,vit_mot); dirmoteur(2,-1,vit_mot); ; Our function'loop()' is pretty basic, first we get the values of the gyroscope and the accelerometer with the method'.getmotion()'. In a second step, we transform and "filter" these raw data with the function filteredpitch(). Then we transform the filtered position into a value that defines the power and direction of the motor. This value is finally used to operate the motors. We will not review the process that allows to recover the data of the gyroscope but we will detail the other functions.

7 _1_ PROGRAMATION float filteredpitch(int AccZ, int AccX, int GyrY,float pitch){ Step 6 // Pitch by the Gyroscope floatgyryd= (float)gyry/ (131); GyrYd= (float)gyryd/ (100); floatpitchgyr = pitch -GyrYd ; // Pitch by Accelerometer float pitchacc = (180/ ) * atan2(accx,accz); // Combination of values pitch = 0.9 * pitchgyr+ 0.1* pitchacc; return pitch; ; Let's go to the explanation of the function 'filteredpitch'. We get two pieces of information, one of our acceleration and one of our gyroscope. The gyroscope gives the movement of our robot in degrees per second. The gyro gives a value of 131 for each degree per second of inclination. We must divide this value by 131 and then by 100 to convert the degrees per second to degrees per 10 milliseconds (because our loops last 10 milliseconds). The same angle is then calculated with the data of the accelerator through process is explained on the next page (180/ π converted radians in degrees). Finally these values are combined giving a different weight to each of them. Angle in /sec We combine these values to obtain a more accurate result. In addition to combining two different sources, this method makes it possible to eliminate a natural tendency of the gyroscope which is to "slide" that is to say to see its value vary without any movement.

8 x y _1_ PROGRAMATION The other information retrieved comes from the accelerometer, it measures the forces that are exerted on our robot in 3 dimensions. For this robot, we will only use two dimensions that we willcallxandy. In stable position, the only force felt is due to gravity, the weight of the robot * the constant: This force is exerted onlyin the y direction of the sensor. During a movement (t = 1) the force of gravity is carriedonboththexandyaxis. t = 0 1g We want to recover here the theta angle (θ), by reminding us some notions of trigonometry, we can determine the angle of inclination of the robot: f2 1g*sin(θ)=f2 sin(θ)=f2/1g Θ=sin-1(f2/1g) OU 1g*cos(θ)=f1 Θ=cos-1*(f1/1g) t = 1 1g f1 y The forces f1 and f2 are the accelerations of the roboton its personal mark (Xa and Ya) returned by the gyroscope. To calculate more precisely this angle we use the arc-tangant which, thanks to the value of the two vectorsf1andf2candirectlycalculatetheangleθ: 1g θ f2 θ f1 x tan(θ) = f2/f1 This calculation gives us the rotation of our robot around the axis z, axis placed in the center of the plane and which points directly towards us (not drawn)

9 _1_ PROGRAMATION Step 7 intpid(floatinput){ input = input ; //cancel the gyro bias floatkp= 250; floatki = 0; intkd=0; floatp = Kp* (float)input; I = Ki * (float)input + I; floatd = Kd* (input -prev_input); prev_input = input; intpid = (P + I + D) ; if( PID > 255 ){PID = 255; if( PID < -255 ){PID = -255; Serial.print(input); Serial.print("\t"); Serial.print(P); Serial.print("\t"); Serial.print(I); Serial.print("\t"); Serial.print(D); Serial.print("\t"); Serial.println(PID); // Serial.println("\t"); return (PID); We will describe here the operation of one of the most useful algorithms in robotics, the PID. PID stands for "Poporational / Integral / Derivative" and this algorithm is used to adapt the power of a motor to the valuesreadbyasensor. For example, for indoor automatic heating, the heating power will depend on the difference between the desired temperature (x) and the current temperature (a). The 3 components of the PID will have a different impact: - P: proportional action to the deviation(x-a) - D: Action related to the change between the last two measuredvalues(a[-1]-a) - I: action related to the sum of the measured values. These values P,Iand D willbe multipliedbyconstants Kp,Ki and Kd which will testify to the importance that we want to give to each of the parameters,theformulawillthushaveaformclosetothis: = + +

10 _1_ PROGRAMATION Ouralgorithmwillthereforerequireavalueininput,forusitwillbetheangle of inclination of our robot. Since the desired angle (the desired value x) is 0, this angle of inclination alone will represent the P value of the PID. Indeed a measured angle of10 will representadifference of10 with respect to the desiredvaluewhichis0. In our program we smooth this value by removing the bias of the gyroscope. When the robot is straight, its gyroscope is not perfectly at 0, here we had to rectify this value by subtracting 15 from our measurement. This value is found by connecting the robot to the computer and observing the measured angle when the robot is straight. The integral is obtained by simply adding the measured value to all previously measured values: I = (float)input + I; Finally, the derivative is calculated by subtracting the value measured by the previously measured value: float D = (input - prev_input); Each of these values is then multiplied by its multiplier (Kp, Ki, Kd) according to the chosen behavior. In our case we will put all K to 0 except the proportional we will initialize to 250. For more stability it is also possible to modify the Kd. These values are those that we used during our montages but yours can be different according to the state of the pile or the surface of the ground. These values are then framed to be between -255 and 255 and the values composingthepidaredetailedinatable.line: Serial.print("\t"); Indicates the column change in an array. Now it remains to insert the function of the engine which is the same as that used in the previous montages.

11 _1_ PROGRAMATION Step 8 void Forward(int speed){ dirmotor(1,1,speed); dirmotor(2,1,speed); void Backward(int speed){ dirmotor(1,-1,speed); dirmotor(2,-1,speed); void dirmotor(int motor,int sense,int percentage){ intpin1,stat1,pin2,stat2,pinp,power; if (motor==1){ pin1=pin1motor1; pin2=pin2motor1; pinp=pinpmotor1; else{ pin1=pin1motor2; pin2=pin2motor2; pinp=pinpmotor2; if (sense==1){ stat1=1; stat2=0; else if (sense==-1){ stat1=0; stat2=1; else{ stat1=0; stat2=0; power=map(percentage,0,255,50,255); analogwrite(pinp,power); digitalwrite(pin1,stat1); digitalwrite(pin2,stat2);

12 _3_ MONTAGE Step 9 Start by embedding the motors in the "Wheel Block". Step 10 Put the "Uno Card" in the "Cardholder" (CF: diagram left), then paste the "Mini- Breadboard" on the "Boardholder" (CF: diagram right). You will be able to hang these parts to the robot later. Step 11 Then insert a "Support Breadboard" equipped with its "Mini- Breadboard" on the "Frame".

13 _3_ MONTAGE Step 12 Now insert two batteries into their lockers, and snap them on the "Battery Holder x2" You can now integrate all previously mounted parts on the "Clipper Central". Step 13 Step 14 All you have to do is add the "Wheels".

14 _3_ MONTAGE Step 15 Now let's go to the connections. We already know the connections of our component L293D, the only difference is that here we will feed directly with a 9v battery. The L293D will resist because it is designed to withstand 36V and 600mA per side. But our engines only draw 300mA maximum. The second connection concerns the gyroscope. The two connections for the current (Vcc and GND) do not present any particular difficulty. The I2C connection is new. Pins A4 and A5 are connected to the SDA and SCL pins, respectively. The choice of these pins is not arbitrary, it is the Arduino pins for this type of communication. Each pin has a particular role, the SDA transmits the data while the SCL plays theroleoftheclock. The gyroscope is powered by the UNO card, itself connected to the computer. We use two circuits to limit interference between the motors, the board and the gyroscope.

15

16

17