WEEK 1

• I will use the NXT Servo that we have on the lab (there are better motors to be used , but i believe this project can work with this motor. If the response is not good , i will change the NXT Servo to a Power Function XL Motor (It's from Lego too - The response is better than the NXT Servos).
• The structure is ready, i'm waiting the Glide Wheel ( to measure angle ) to finish the building instruction and parts list and start coding.
• There is a extension to Simulink that allows to connect the NXT Brick to Simulink and run the balancing code from the Simulink - I'm reading about it . But the Focus will be coding in NQC and running it

ITENS ORDERED Glide Wheel

OPTIONAL ITENS (I WILL NOT ORDER YET) XL Motor Battery Box for XL Motor

WEEK 2

• I finished all the building and i have all the necessary sensor and motor to start coding.We had at the lab the Power Function E- Motor , which is weaker than the XL Motor , but is better than the standard RCX servo.
• I'm start coding this week

WEEK 4

I'm trying to tune the gains. I will have to modify a few things from the original project:

• I will have to add more beams to make the structure more stable
• I will probably need to buy one cable extension or build a platform to put the brick next to the Angular sensor ( because the cable of the two Angular sensors are very small )
• The cable of the angular sensor attached to the pendulum is very stiff , so when the shaft attached to the motor is rotating , depending on the cable position , the cable offer more “resistance” to the movement and i think this is causing some instability.
• I'm working with one Angular sensor and one Motor controller with angular sensor(which is also a angular sensor ). The Angular Sensor (The red one on the picture of week 2) is not supported by the simulink extension , only the Angular sensor with the motor controller ( the black one on the picture of week 2) is supported.
• The Power Function E-Motor have a good performance but i imagine that the Power Function XL-Motor would have a better performance.

Control Method: I'm programming only on the Bricx program ( that we used on class ) . I'm using the classical PID on the Angular sensor attached to the pendulum , measuring the angle , calculating the velocity and the errors. To run the motor i'm using foward command or Rev command using the power as a function of the PID gains and its respective errors. I tried to use PID on the motor also , ( by hand and using the RotateMotorExPID - which allow to control the motor rotation using PID gains ) but the performance got worse, so i got back to only the OnFWD and OnREV commands.

Videos of progress: Analyzing the videos my guess is the derivative gain need to be adjusted , but i'm working on it a few days and the performance is not getting better. The pendulum is travelling a long circular distance in order to keep balancing ( first video ). The second and third videos shows a little bit of the derivative gain adjusting , i tried small and big values , but i could not find the perfect value. Testing 1:

Testing 2:

Testing 3:

WEEK 5

LQR Approach → I'm following two different papers to find the LQR derivation for a rotary inverted pendulum. The state space model from the two papers are different , and i found two different [K] matrix for my model . ( changing the model parameters to my parameters and using Matlab to solve the ARE and get the [K] matrix ( I'm using Q as diag(100,1,100,1) and R=1 - plausible values and the pole placement is OK on both). However , the two [K] matrix were not good ( we always need some kind of hand tuning after finding these gains ). Since then i'm trying to tune the gains from the model that gave me the best balance.I tried to use the LQR gains from the MathWork program but the response was not very good.

I'm now using a full state feedback ,using the gains from LQR. I found a bug on the AngularSensor with motor controler - if you start the MotorRotationCount and rotate the motor to the negative direction , it will give sometimes a very wrong reading . So i had to insert on the code a 360 degree rotation on the motor before the system start working to use the Motor position as the (Actual counter - 360) to avoid these kind of wrong readings.

I found that limiting the motor maximum power to around 40~~50 of maximum power the system balance better. ( using saturation -100 x 100 the motor vibrates a lot when changing the turning direction). I tried to apply a filter on the velocity calculation (using average from 4 measures) but the system performance does not improved. Other strange thing happening is that sometimes in the middle of the balancing i got a “BlackOut” and the system loose the balance ( see video) . I don't know yet what can be causing this.

MATLAB CALCULATION

FIRST PAPER

SECOND PAPER wjet_2015052217370851.pdf

BALANCE SITUATION: Here is shown the “BlackOut”. I have been trying to tune the gains since Thursday, i don't know if i'm already at the better performance or not , i need to get the New Angle Sensor and run this setup on the simulink and see if the motor is limiting the performance or what is wrong with the code.

Difference between my model and the available on the internet → The control method is pretty similar to the majority of papers that i found on the internet - Full state feedback using LQR to find the gains. One thing that the guy from MathWork ( and Prof Kawata) did different is the method to derive the velocity. The guy from MathWork uses transfer functions blocks to find the velocity (input angle , output velocity). Prof Kawata is using the StateSpace block model to derive the velocity (input angle , output velocity ). And i'm using the manual method (Angle-PreviousAngle)/dt , dt= interval between the control loop. The Motor Input we are all using the same ( [K]*[x] ) (x as the matrix with the two angles and two velocities).

Next steps

• I sent the email with the parts i need to Paresh and he ordered the parts monday, but they did not arrive at the lab yet ( friday ). When i get the parts i will:

• First i will run the MathWorks model with the E-Motor and see what was the problem with my code. If the Simulink model could balance the pendulum with this motor after trying to tune the LQR gains the problem is my code. If the Simulink model could not get close to a balance , the problem should be the motor.

• After, i will replace the E-motor for a XL-motor and try to run the Simulink model. (the pendulum length is the same and the Brick the same , so the Gains should be very close to the tuned gains). I will find the gains that balance in the Simulink model and i will use this gains on the NQC code and see what happens (if the code is OK , the balance should be good on NQC too). If the balance is not OK the code has something wrong.

WEEK 6

• Day1 : I'm still waiting the parts so today i spent more time refining the gains and modifying a few things on the code. I was looking at the Simulink model from Professor Kawata and he is using a different input equation. He uses the traditional [K]*[states] but he adds another factor ( a constant times the signal of the arm velocity ). This can be the reason that his model is balance way better than the MathWorks project.

Voltage Calculation. ( the standard [K] from LQR times the Matrix with the states )

factor:

• Day 2 and 3. No progress. The sensor already arrived at the lab but i'm still waiting the XL- Motor. Tomorrow (Thursday) the motor should arrive and i will start the final tuning of the system. During the weekend the project should be finished!
• Day 4 and 5: Already got all the necessary parts to build the final structure. The final structure is done. The balance with the XL-motor is way better than the E-motor. The XL motor is way more powerful and robust (I could easily rotate the E-motor by hand , and the XL motor is pretty difficult to rotate it by hand). The XL motor oscillates less when changing the rotating direction. I got a good balance using Simulink and the balance using NQC is getting better. I identified two things that the NQC code needs to improve. One is the velocity calculation , i will have to find a way to apply a filter , without loosing much processing time. And looks like the time between each loop on the Simulink is a little bit faster than the NQC (I'm using a function to get the sensors data , i will try calculating all the sensors data in a separated task and using global variables - this values can be available at all time).

• Day 6: I'm trying to find what is limiting the balancing on the NQC code. The Simulink code is running smooth ( see video ). I'm using the same algorithm on the NQC code. What i did today:
• 1→ I tried first with my original code to do a little gain tune , because the Simulink gains did not work with the NQC code.I could not tune the 4 gains to improve the balance the way it was needed .So i start to look for possible mistakes on the code.
• 2→Testing the interval between loops: Using a single task and a single function ( void ) returning the pendulum and motor angle and position the time between loop was 2.2ms . Using two tasks , one for balance and one to get the pendulum and motor angle and position the time between loop was 5.8ms. I'm now using a single task to balance and two functions ( one to handle the pendulum variables and one to the motor variables). The time between loop sounds OK .
• 3→I checked if my references was the same of the Simulink model (Positive/Negative turning direction of the sensors ). It was everything OK , so in theory using the same gains the balance should be pretty much the same.
• 4→Checking the input sent to the motor: I made some test and i observed that even using the same gain to the Pendulum position and the motor position , the input sent to the motor was a little different ( maybe the precision of the variables -float on the NQC and double on the Simulink - and the rouding methods - the input sent to the motor is integer.

• 5→I tried to fix the input issue: first i tried to do a gain tune to make the two input to be the same on the same conditions ( same motor angle and pendulum angle) - NO SUCCESS , always a little difference (1~3). But at least the difference got a little smaller.I had to change some variables type ( ex: on the Simulink , the block that get the angle send an integer value , and i was using a float variable on the API to get the angle ( using this sensor from mindsensors the API is the same as a regular motor (MotorRotationCount).
• 6→With the time between interval OK and the Positions gains “OK” , i tried to do a gain tune again to make the balance better - no major changes. So i jumped to the next problem , the velocity calculation.
• 7→I started to change the methods to calculate the velocity. I) using the dt as the CurrentTick - PreviousTick (API's from the Bricx program ) II) Calculating dt with a function to handle the time between loops ( similar to the code of the WhipBalance that we used on class ). III) Calculating three values in a row and taking an average. IV) I'm now working on using the average of previous values (similar to the code of the WhipBalance that we used on class), i will use an average of 5 previous values to calculate the velocity.

USING SIMULINK

Day 7:

• I'm now calculating the velocity using the average from 4 previous values and the balance got significantly better but still not as good as running on Simulink (using the exactly the same gains). The balance can get better with a little gain tuning - which i'm trying right now. I thought that the interval between loop was OK , but i think this can be causing the Simulink to balance better ( see the two videos on the Building Tutorial Tab ) .Another thing that is different is the motor behavior . Running on Simulink we can't notice the motor “input spikes ” , it accelerate and break very smoothly . Runing on NQC we can notice the motor “input spikes” . For example , using the Simulink it looks like if the motor was turning foward with power of 40 and it needs to change to 60 , it will accelerate and break to reach the new value , and using the NQC code it looks like if the motor was turning foward with power of 40 and it needs to change to 60 it will change from 40 to 60 instantly - so we can notice the “input spike” . There is an API on the Bricx that can control the OnFwd and OnRev using PID constants, however i tried to use it and it does not change the balance at all.

WEEK 7 Now that the NQC is running smoothly the next steps are:

1. Finish the swing up control.
2. Fill up the spreadsheet with the necessary parts for the RIP Box.
3. Do the a detailed version of the building instructions
4. Do the step - by - step coding in the Bricx Command Center
5. Explain the theory behind the NQC code
6. LQR Theory - how to find the gains