RC SERVO ROBOT ARM: Weekend project

This is our weekend project: a simple RC servo robot arm. 

Host controller is Mac using Python scripting, while servo controller is a spare Fubarino SD.

I tried with MSP430 Launchpad, but Serial + 3 servos was over the capability of this little Mac-hated board. Fubarino is a cherry plenty of power, and the ported SoftwareServo library from ChipKit is stable and effective.
Fubarino accepts directly joint angle in ms. Python script uses a simple Inverse Kinematics (Updated versione: math credits to COŞKUN YETİM):

def ik(self, position):
#ARM LENGHTS
l1 = self.arm1
l2 = self.arm2

#END EFFECTOR POSITION IN RESPECT OF ORIGIN NODE
x = position.x - self.home_position.x
y = position.y - self.home_position.y

#TO LIMIT END POSITION WITHIN ARM REACHABLE CIRCLE
dist = min(l1+l2, sqrt(x*x + y*y))
dub = sqrt(x*x + y*y) / dist
x /= dub
y /= dub

#SIMPLE, NO?
thetar = acos(x/dist)
theta1 = acos((l1*l1 + x*x + y*y - l2*l2) / (2*l1*dist)) + thetar
theta2 = pi - acos((l1*l1 + l2*l2 - (x*x + y*y))/(2*l1*l2))

self.set(180-degrees(theta1), 180-degrees(theta2))

Graphic is provided by a simple Svg to path script. The library used (svg.path) can provide point coordinates for Line, Arc, CubicBezier and QuadraticBezier without pain. All graphic may be created within Inkscape.


Overall code may be downloaded at this link.

BOXEDCNC: Brushless ESC pwm response, linearizing output for PID control, and smoothing the sensor values

Once assembled and tested, i had a lot of data collected from brushless spindle controller, i've used to implements a better control method, a PID control. I've spent long time updating the code to find what was going wrong. Regarding PID, there are a lot of good articles in the web, some implemented also on the Arduino universe. Start your readings from the incredible series of post from Brett, if you want to know more about PID.

In this post i want to describe my solution to some of the headache i had to implement:

1. Keep it fast if what you want to control react fast (a spindle is really different from sous-vide or temperature PID room controller). My timing between refresh is now 50ms.
2. Keep it steady, eliminating every distraction from the PID computation (like delays).
3. Linearize the output of the PID, before applying to the ESC to get rid of the PID constants.
4. Try to guess the output value, before enabling PID.
5. Divide the PID constants in aggressive and conservative variants depending from the error between desired set point and actual reading.
6. Don't try to find a magic method to find constants. Try various values starting only from progressive, then enabling integrative, and only at last using derivative.

The next image shows the schematic of the brushless controller. There are four sensors in the system, but the fourth is not showed because is a temperature sensor.

The main loop in CNC mode use the GCode Spindle commands (like S5000) as target value to control with PID in a closed loop the motor RPM, trying to minimize the error between target and actual speed.

The major drawback i discovered using the affordable Turnigy ESC is the non linear response of the motor's rpm in the basic free spinning condition (without torque applied). As you can see in the next image, the RPM relation between Arduino PWM uS interval and motor RPM is something similar to a quartic function. I think this is quite common when using the writeMicroseconds function approach. In fact you can control the servo with high precision writing an interval typically between 1000uS and 2000uS. In the next graph is possible to see the real data registered regarding RPM and the Power Source output tension and current given to the ESC. Without any resistance applied to the motor's  shaft, the ESC absorbs circa 70W at the maximum speed. The power source drop voltage between zero and maximum speed is only 0.30V (from 12.87V to 12.50V).

To avoid strange PID behaviors, i needed to implement a fast transformation function. 

The rest of the post is about how i've implemented a SMA (Simple Moving Average) to smooth the RPM readings, and about the linearization of the output of the ESC.

The start is a simple moving average using integer array to take care of the fast RPM refresh process, without dedicated onboard hardware. This was software implemented using a simple array which hold a certain number of integer numbers (from 30 to 50). These values are the microseconds between two consequents falling edge (or raising, is the same) of the rpm optical sensor, collected on   interrupt pin. This happen continuously with a period in a range from 2.000 and 15.000 uS. The array index is incremented every interrupt, and is zeroed using the modulo operator in one code row.

The result is a Simple Moving Average of the continuous RPM data, which is implemented using  this code in the interrupt function:

void rpmMeter() //INTERRUPT CALLED FUNCTION

 {

   RPMIndex = (RPMIndex +1)%(RPMIndexSize-1); 

   RPMTimes[RPMIndex] = micros() - RPMTimeold;

   RPMTimeold = micros();

 };

Once 50ms i compute the sum of the array, and divide the result by number of elements in the array to obtain a smoothed RPM fresh value.

The next step is to transform the output from the horrible quartic function to a simple linear response.

This happens using another array containing the uS timing values at which corresponds the desired output. I think this is a fast method because the loop only need to lookup in a prewritten array using the index as input.

The next graph shows the test results without smoothing (real data). In the x axis there are the values of the linearized output range, in the y axis the RPM logged.

BOXED CNC: Brushless Spindle & Controller, MpIde sketch

Di seguito il link allo sketch per il Fubarino SD (MPIDE) da scaricare:

Download link: PDE SKETCH MPIDE

Richiede le seguenti librerie standard:

• Servo.h
• LiquidCrystal.h
• Bounce.h
• PID_v1.h
• stdlib.h:

I pin utilizzati:

• LCD RS = 10;
• LCD E = 11;
• LCD DB4 = 12;
• LCD DB5 = 13;
• LCD DB6 = 14;
• LCD DB7 = 15;
• VOLTAGE SENSE = A1
• CURRENT SENSE = A3
• RPM INTERRUPT = 4
• SPINDLE PWM = 9

BOXED CNC: Brushless Spindle & Controller

Per le vacanze di natale ho fatto passi avanti su un progetto che avevo in mente da tempo: la sostituzione del motore della MF70 con un motore brushless.

Un ringraziamento per holcon1 che ha individuato un motore adatto alla conversione: Youtube video

Il sistema è composto da un motore brushless (Hobbyking NTM 3536 - 1800kV - 875W), pilotato con un Turnigy Plush 80A e comandato da un Fubarino SD (un micro Arduino-inspired con un potente PIC a 32 bit, 80Mhz) ed alimentato da un alimentatore 12V da 85A inizialmente destinato ad un server. La velocità massima attesa è di 22kRPM, poco superiore a quella originale della MF70.

Il controller ha 3 modalità di funzionamento, stand-by, manuale e cnc (accetta comandi via usb).

Per la modalità manuale c'è un pratico e preciso potenziometro multigiro (non si può abilitare il motore se il potenziometro non è a zero). Per la modalità CNC sono accettati i comandi standard per l'accensione dello spindle, impostazione della velocità, stop dello spindle.

Completato un sensore di tensione con un voltage divider, da completare il sensore di corrente e RPM (per il PID): ho calibrato la lettura confrontandola con il multimetro digitale.

Per far sapere alla PSU di fornire i 12V è stato necessario collegare due (Enable e Voltage Sensing).

Ho scoperto che l'albero del motore ha un diametro di poco inferiore ai 4mm ed il pignone standard della MF70 non calza bene (troppo grande). Al momento ho provato a brasarlo e fissarlo, ma temo che la tenuta meccanica non sia eccezionale. Di seguito anche un adattatore che ho fatto al volo con il vecchio motore da un pezzo di ottone da 10 mm di spessore.

Quando potrò ordinare il kit mandrino della Usovo (ER11 Collet adapter) spero si possa risolvere diversamente l'accoppiamento.