问:
Remote Control Relative to Driver
Is it possible to remote control a 'robot' relative to the driver with an angle sensor (or any other sensor)?  For example, if the robot starts in this position

```<code>--------------
|    Front   |
|  --------  |
| |________| |     [robot]
|    Back    |
--------------
</code>```

and the joystick is in this configuration

```<code>--------------
|  Forwards  |
| [joystick] |
|  Backwards |
--------------
</code>```

then if the robot turns around,

```<code>--------------
|    Back    |
|  --------  |
| |________| |     [robot]
|    Front   |
--------------
</code>```

pushing the controller forwards will still make the robot go forward

```<code>--------------
|      ^     |
| [joystick] |
|  Backwards |
--------------

--------------
|      ^     |
|  --------  |
| |________| |     [robot]
|    Front   |
--------------
</code>```

even though from the robot's POV, he's going backwards.

Any ideas/solutions?

答:
Sure, as along as you can measure orientation (compass, GPS, whatever). Remember the initial orientation and then before figuring out the direction to command the robot, you just need to rotate the coordinates. So if the joystick (DRIVER) and robot are aligned to start (initial offset 0) and then the robot is pointing, say at 90 degrees, and you command the robot to move in the 180 direction FROM THE DRIVER PERSPECTIVE, you take the 180 minus the current orientation of the robot (90) and issue a command for the robot to move in the 90 degree direction IN IT'S OWN ROTATED COORDINATE SYSTEM.

So robot command is driver coordinate command direction minus current robot direction relative to the driver coordinate (which was stored at initiation). Just add one more factor if the robot and driver aren't initially aligned.

Title: Remote Control Relative to Driver


问:
the specifications graph showing the battery discharges in volt per time
my name is dylan we are doing a project on irobot create and we would like to know the specifications graph showing the battery discharges in volt per time and my robot is the irobot ceate 1.

The battery is the roomba advanced power , it's a 14.4V Nickel metal hybride battery pack and she deliver 3000mah.

答:
There are a number of factors that affect battery voltage of NiMH batteries during discharge versus time.  Discharge rate, battery temperature, and battery age are all major contributing factors.  As a result there is no one graph of battery voltage vs. time, there are usually a family of curves.

You could characterize the battery you have by measuring the voltage at specific time intervals during a discharge and build the graph yourself.  Be aware that if you change the load current you will need to make a new characterization.  As the battery ages the internal resistance increases so your initial characterization will become progressively inaccurate.

How do you plan to use this information?

ContentLicense: CC BY-SA 3.0


问:
Basic general question about controllers
Suppose I have a mechanical system which is free to move on a given rail [-5m, 5m] like a motorized cart. The whole system can be mathematically expressed through a linear timeinvariant system equations.

If I need to control only the position (for example saying the controller: "move to +2.3") I can simply design a PID controller that, given a set point moves the cart to that position.

Now I need much more and I want to control the position and the velocity of the cart. So I need for example to say: "move to +2.3 with a specific velocity's profile". Of course the vel = 0 at the end position.

Question: how should I design such a controller? Do I need specifically a special type of controller? Or I have a huge choice?

Any help, graph, link and example is really appreciated.

Regards

答:
The best control you can design is "exact feed forward linearization based on differential flatness". This is a complete approach to design control laws for flat systems.

Just PID is not a good idea : you will get overshoot and you do not master precisely position/velocity.

Flatness is a generalization on controllability. Hence you are sure that your system can achieve any position/velocity profile provided you remain inside the physical constraints.


问:
Why does our LM2576 circuit suddenly cut down the power?
I have an LM2576 circuit plus an adjuster to adjust the output voltage, for controlling motor speed in a line follower robot. The circuit works great when adjusted to give out low voltages, but when I adjust it to higher voltages for my motors to go faster, it works great for 1-2 minutes, then suddenly cuts down the power and motors start to go extremely slow.

Even when I decrease or increase the output voltage, it won't respond until I turn off the power and turn it back on again. There is something mentioned in the LM2576 datasheet that if we overload the IC it will cut down the power until the load comes lower, so I think it might be a problem with that.

Since this problem has already caused us to lose the competitions with 5+ teams, I would like to solve it for our next competition, so why does our LM2576 circuit suddenly reduce the power?

答:
If I were you, I would first (1) read the LM2576 datasheet.
I'm assuming you are using a circuit similar to the schematic and PCB layout on page 23 of the LM2576 datasheet.
I'm guessing you've tweaked the circuit slightly, replacing the manually-operated pot shown on the schematic for R2, replaced with some sort of microprocessor-controlled thing that frequently changes its effective resistance to make that motor spin faster or slower.

Then I would (2) put my finger on the chip.
If it feels so hot that I can't hold my finger on it, I would suspect

thermal shutdown.

georgebrindeiro covered this. You'll also want to read p. 18 of the datasheet, the section "Thermal Analysis and Design": "The following procedure must be performed to determine whether or not a heatsink will be required. ...". The typical solution is to add a big heat sink to the chip. Do you see the size of the heatsink on the PCB layout on page 23 of the datasheet?

Next I would (3) take a cheap multimeter, switch it to "Amp" mode, and connect it in-line with the motor leads.

When the output voltage drops (which I measure with my more expensive multimeter),
and I hear and see the motors slow down, what does the cheap multimeter say?

Does the cheap multimeter stay pegged at some high current above 3 A (the datasheet says the internal limit will be somewhere between 3.5 A and 7.5 A)?
If so, then we almost certainly have:

current limiting. The regulator is working as-designed.

The typical solution is to figure out what is pulling so much current, and somehow replace it with something that pulls less current.
Perhaps the robot has run into a wall or got stuck in a rut, and the motors have stalled out.
Then maybe the controller needs to sense this condition, stop its futile efforts to punch through the wall, and reverse direction.

Sometimes we really need more current than one regulator can supply.
Then we must replace that regulator with multiple regulators or higher-current regulators or both.
(But not so much current that it immediately melts the wires in the motor).

On the other hand, if the output voltage and the output current drop, then
I would (4) connect my more expensive multimeter to the power input pins of the regulator.
If that is much lower than I expected, then perhaps:

The input voltage is lower than I expected.

When you supply the motors with a higher voltage, they drain the batteries much quicker. Partially-drained batteries have a no-load voltage lower than you might expect from the "nominal" voltage printed on the battery, and loaded batteries have an even lower voltage. I doubt this is your problem, since you imply that if you disconnect the battery and reconnect the same battery, it seems to start working again. The typical solution is to put more batteries in series to increase the actual working voltage.

Next I would (5) try disconnecting the batteries, and powering everything from a high-current grid-powered "power supply" set to the appropriate output voltages.
If it seems to run fine off that power supply, but not from batteries, I would suspect:

switching-regulator latchup. "Latchup of Constant-Power Load With Current-Limited Source" http://www.smpstech.com/latch000.htm

When you supply the motors with a higher voltage, they pull a higher current.
Surprisingly often, the total impedance at the input power pins of switching regulator is so high (bad) that when the regulator turns its internal power switch on, the voltage at its input power pins plummets so low that it's not possible to pull the desired current from that battery.
Milliseconds later, when the switch turns off, the battery pulls the voltage back up to some reasonable voltage -- so this problem is difficult to debug with a standard multimeter, but obvious when you have the input power pins connected to an oscilloscope.

The only complete cure is to somehow reduce the total input impedance at the input power pins.
Occasionally all you need to do is reduce the wiring impedance -- shorten the wires (reduce the resistance), or bring closer together the PWR and GND wires (reduce the inductance), or both.
Occasionally all you need to do is put more capacitance across the power input pins of the regulator.
As a battery drains, its effective series resistance increases.
In theory, you can always cure this by putting more batteries in parallel to reduce the net ESR of of all the batteries in parallel.

Some people want a very lightweight robot, and so they can't add more batteries, so they can't completely cure the problem.
Sometimes they can get adequate performance from the robot by using various "soft-start" or "load-shedding" techniques.
Rather than trying to pull a lot of power from the batteries to get up to speed quickly -- more power than the battery can physically supply, and so triggering this unwanted latchup -- we pull somewhat less power from the batteries, slowly ramping up to speed, and applying various "limp mode" and "tired mode" techniques to keep the total power at any one instant low enough that the battery can supply it.
(You may be interested in
What is the best way to power a large number (27) servos at 5 V?
).


问:
Is it tough to make a robotic workshop of your own
I want to make a robotic workshop.. I recruited 10 members to work...  Please give some tips about robotic workshop</p>

答:
I'm going to completely disagree with previous answers stated. Robotics consists of two major parts: 1.Hardware and 2.Software. Mechanical and  electrical are generally subclasses of hardware. Software is a subclass of, you guessed it, software. Software encompasses programming of course, however some argue that it also encompasses algorithm analysis and computational processing.

Nonetheless, the difficulty of starting a robotics workshop, as you call it, depends on the skills and backgrounds of the people you recruited. If everyone is a software engineer, you're going to have problems, the same applies for if everyone is a mechanical engineer. The hardest part of robotics comes from  understanding how all the encompassing disciplines and their respective principles come together to function in a single system. You have to be able to look at individual  subsystems and then understand how these subsystems interact with each other to create the main top-level system.

Anyway digressing from that tangent, if you are looking for a starting point look at arduino and at raspberry pi. The great thing is that they are open source, so you can look at other projects, make your own, and ask for help here and on their very active respective forums. Look at a website called seedstudio.com it has a large variety of arduino and raspberry pi compatible parts you can order and use for whatever you want.

As for programs, for 3d cad user solid works or autodesk inventor. For PCBs eagle or tinycad are good. For programming the arduino ide or visual studio are good. And there's a site called opensourcehardware.com that has all the tutorials on arduino programming and interfacing with digital electronics. I would definitely encourage you to look at those.

Good Luck.


问:
aerodynamics of quadcopter
I want to build a quad copter. I want to know how do we calculate thrust or lift generated by using a motor, I am not aware about the capacities of motor. So can you explain how to calculate thrust or lift generated by assuming a motor. And what is the maximum payload that a quad copter can lift for a given thrust. </p>

答:
This is actually quite complex and depends on a fair number of factors. You will need to know the specs of the motor, especially kV, but also its expected current draw and the internal resistance. Obviously, your choice of propellers will also have an effect, and so will your choice of powerplant. It also depends on the ambient air temperature and pressure.

There are online calculators that can help you in that regard, this one lets you pick some common motor models and has a lot of other variables to consider. It offers feedback that includes lifting capabilities, engine efficiency and heat generated by the motors, among other things.

It should also be noted, that the theoretical values from the calculator can be quite different from the actual values you'll see in reality, but its a good starting point for helping you choose suitable equipment.

Title: aerodynamics of quadcopter


问:
Controlling an ESC for Brushless Motors with an RPi
I'm looking to build a new (first) quadcopter without the conventional flight controller and radio, with an onboard RPi and applying some newfound knowledge on autonomous control to improve my coding skills.

Although, since I've never actually built a quadcopter, I don't actually have any experience in using brushless motors.

I'll be using a RPi B+, so controlling them over I2C was something I looked into. The B+ though only has two I2C interfaces. It also only has two hardware PWM pins and I'm unsure whether software PWM would be enough. I found the Afro SimonK-based ESCs from HobbyKing which have I2C (Intended for the MikroKopter).

I've looked around and people have used the Adafruit 16-channel PWM/Servo drivers to control them. Is this an option to look into? Or is there perhaps a better way?

Also, would be it particularly safe if the RPi is run off the ESC's BEC? It's confusing because, when the ESC is powered on, well, it'll be powered on before the RPi comes up. What do ESCs do when they have bad input?

答:
Operating an ESC is practically the same as operating a servo. The main difference is that instead of the pulse width translating to a position  on the servo, it translates to an output speed. The range of widths is identical with ~1000µs representing the lowest setting and ~2000µs representing full on.  These values can sometimes go beyond (as overshoot), but those are usually considered the 0% and 100% values.

I would search for any sort of servo libraries for the RasPi, they will do the trick. Using an additional servo controller though is always an option.

It is completely reasonable to use the BEC to power your electronics.


问:
What is the link between a quadcopter transmitter pulse and the roll/pitch/yaw angles?
I want to design a data logger for my quadcopter using the Arduino Mega board. I want to record the roll, pitch and yaw angles each second or 5 seconds, so they can be viewed later after a flight has ended. There's just one thing I don't understand, and that's how to translate the pitch/roll/yaw angles into a pulse of a specific length that the flight controller receives.

For example, when I press the control for the pitch, the transmitter sends out a pulse to the receiver of the drone and the speed of the drones' motors change accordingly for it to pitch either forward or backward. I can tap into these commands between the flight controller and the transmitter, and be able to record the length of the pulse that was sent out. However, what is the link between the pitch angle and the size of the pulse? Basically, how can I convert the pulse that was recorded by the Arduino board and convert it into the pitch angle in degrees?

Generally, for the transmitter I use, a 1500us-pulse means zero pitch; from 1501-2000 means pitch forward, and from 1000-1499 means pitch backwards (of course, the actual values vary slightly, but this is just a general reference for this question). So for instance, if I sent a pulse of 1400us, how would that translate into an angle in degrees? What's the formula to convert it?

I hope I'm clear, and if this question sounds stupid, please excuse me, but I haven't been able to find good information on it!

Thanks!

答:
There isn't exactly a predictable correspondence of the two. I mean, you've already said that 1500us is the mid point. Values above or below that are really just amounts of pressure to apply to the model in order to move it. The actual angle will depend very much on how you have tuned your PID, but will also be influenced by the wind and the weather.

All you can really do is log the values of the width of the pulse which you can translate to how far the stick has been moved. Beyond that I wouldn't rely too much on 'angles'.


问:
how to implement tracking problem with PID controller
I'm trying to implement the tracking problem for this example using PID controller. The dynamic equation is

$$
I  \ddot{\theta} + d \dot{\theta} + mgL \sin(\theta) = u
$$

where

$\theta$ : joint variable.

$u$ : joint torque

$m$ : mass.

$L$ : distance between centre mass and joint.

$d$ : viscous friction coefficient

$I$ : inertia seen at the rotation axis.

$\textbf{Regulation Problem:}$

In this problem, the desired angle $\theta_{d}$ is constant and $\theta(t)$ $\rightarrow \theta_{d}$ and $\dot{\theta}(t)$ $\rightarrow 0$ as $t$ $\rightarrow \infty$. For PID controller, the input $u$ is determined as follows

$$
u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \underbrace{0}_{\dot{\theta}_{d}} -  \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d} - \theta(\tau)) d\tau
$$

The result is

and this is my code  main.m

```<code>clear all
clc

global error;
error = 0;

t = 0:0.1:5;

x0 = [0; 0];

[t, x] = ode45('ODESolver', t, x0);

e = x(:,1) - (pi/2); % Error theta

plot(t, e, 'r', 'LineWidth', 2);
title('Regulation Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\theta_{d} - \theta(t)$', 'Interpreter','LaTex');
grid on
</code>```

and ODESolver.m is

```<code>function dx = ODESolver(t, x)

global error; % for PID controller

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;

% u: joint torque
u = Kp*(pi/2 - x(1)) + Kd*(-x(2)) + Ki*error;
error = error + (pi/2 - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

end
</code>```

$\textbf{Tracking Problem:}$

Now I would like to implement the tracking problem in which the desired angle $\theta_{d}$ is not constant (i.e. $\theta_{d}(t)$); therefore, $\theta(t)$ $\rightarrow \theta_{d}(t)$ and $\dot{\theta}(t)$ $\rightarrow \dot{\theta}_{d}(t)$ as $t$ $\rightarrow \infty$. The input is

$$
u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \dot{\theta}_{d}(t) -  \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d}(t) - \theta(\tau)) d\tau
$$

Now I have two problems namely to compute $\dot{\theta}_{d}(t)$ sufficiently and how to read from txt file since the step size of ode45 is not fixed. For the first problem, if I use the naive approach which is

$$
\dot{f}(x) = \frac{f(x+h)-f(x)}{h}
$$

the error is getting bigger if the step size is not small enough. The second problem is that the desired trajectory is stored in txt file which means I have to read the data with fixed step size but I'v read about ode45 which its step size is not fixed. Any suggestions!

Edit:

For tracking problem, this is my code

main.m

```<code>clear all
clc

global error theta_d dt;
error = 0;

theta_d = load('trajectory.txt');

i    = 1;
t(i) = 0;
dt   = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
   i = i + 1;
   t(i) = t(i-1) + dt;
end

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(@ODESolver, t, x0, options);

 e = x(:,1) - theta_d; % Error theta

plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on
</code>```

ODESolver.m

```<code>function dx = ODESolver(t, x)

persistent i theta_dPrev

if isempty(i)
    i = 1;
    theta_dPrev = 0;
end

global error theta_d dt ;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 35.5;
Kd = 12.9;
Ki = 1.5;

if ( i == 49 )
    i = 48;
end
% theta_d first derivative
theta_dDot  = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);

% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

i = i + 1;
end
</code>```

trajectory's code is

```<code>clear all
clc

a = 0:0.1:(3*pi)/2;

file = fopen('trajectory.txt','w');

for i = 1:length(a)
    fprintf(file,'%4f \n',a(i));
end

fclose(file);
</code>```

The result of the velocity is

Is this correct approach to solve the tracking problem?

答:
I've made few changes to your ODESolver:

The time step requested by ODE45 is not going to match what you have in your file. In your ODESolver() desired thetas are read one after the other and then the last one is repeated. As a result the desired theta was not a function of $t$. I used interp1() to fix that.

The time difference between two iteration of ODE45 is not constant so you can't use dt to calculate the $\Delta{\theta}$. This is fixed by storing the t in a variable for future reference.

Same goes for calculating the integral part of PID.

Here is the new ODESolver():

```<code>function dx = ODESolver(t, x)
persistent theta_dPrev time_prev

if isempty(theta_dPrev)
    theta_dPrev = 0;
    time_prev = 0;
end

global error theta_d dt time_array;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 40;%35.5;
Kd = 10;%12.9;
Ki = 20;%1.5;

% theta_d first derivative
theta_desired_current = interp1(time_array,theta_d,t);
if t==time_prev
    theta_dDot = 0;
else
    theta_dDot  = ( theta_desired_current - theta_dPrev ) / (t-time_prev);
end
theta_dPrev = theta_desired_current;

% u: joint torque
u = Kp*(theta_desired_current - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_desired_current - x(1))*(t-time_prev);

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

time_prev = t;
end
</code>```

I also changed the main.m to the following:

```<code>clear all; close all; clc;

global error theta_d dt time_array;
error = 0;

theta_d = load('trajectory.txt');
% theta_d = sin([1:.1:20]/3);

time_array=0:dt:dt*(numel(theta_d)-1);

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t_ode, x] = ode45(@ODESolver, [time_array(1),time_array(end)], x0, options);

theta_desired_ode = interp1(time_array,theta_d,t_ode);
 e = x(:,1) - theta_desired_ode; % Error theta

figure(1)
plot(t_ode, x(:,2), 'r', 'LineWidth', 2);
title('Velocity','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on

figure(2)
plot(t_ode,x(:,1))
hold on
plot(t_ode,theta_desired_ode)
title('Theta','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
legend('Actual Theta', 'Desired Theta')
grid on
</code>```

Body:
I've made few changes to your ODESolver:

The time step requested by ODE45 is not going to match what you have in your file. In your ODESolver() desired thetas are read one after the other and then the last one is repeated. As a result the desired theta was not a function of $t$. I used interp1() to fix that.

The time difference between two iteration of ODE45 is not constant so you can't use dt to calculate the $\Delta{\theta}$. This is fixed by storing the t in a variable for future reference.

Same goes for calculating the integral part of PID.

Here is the new ODESolver():

function dx = ODESolver(t, x)
persistent theta_dPrev time_prev

if isempty(theta_dPrev)
    theta_dPrev = 0;
    time_prev = 0;
end

global error theta_d dt time_array;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 40;%35.5;
Kd = 10;%12.9;
Ki = 20;%1.5;

% theta_d first derivative
theta_desired_current = interp1(time_array,theta_d,t);
if t==time_prev
    theta_dDot = 0;
else
    theta_dDot  = ( theta_desired_current - theta_dPrev ) / (t-time_prev);
end
theta_dPrev = theta_desired_current;

% u: joint torque
u = Kp*(theta_desired_current - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_desired_current - x(1))*(t-time_prev);

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

time_prev = t;
end

I also changed the main.m to the following:

clear all; close all; clc;

global error theta_d dt time_array;
error = 0;

theta_d = load('trajectory.txt');
% theta_d = sin([1:.1:20]/3);

time_array=0:dt:dt*(numel(theta_d)-1);

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t_ode, x] = ode45(@ODESolver, [time_array(1),time_array(end)], x0, options);

theta_desired_ode = interp1(time_array,theta_d,t_ode);
 e = x(:,1) - theta_desired_ode; % Error theta

figure(1)
plot(t_ode, x(:,2), 'r', 'LineWidth', 2);
title('Velocity','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on

figure(2)
plot(t_ode,x(:,1))
hold on
plot(t_ode,theta_desired_ode)
title('Theta','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
legend('Actual Theta', 'Desired Theta')
grid on


答:
Since both $\theta_d\left(t\right)$ and $\dot\theta_d\left(t\right)$ are references at your disposal, i.e. you have to provide them in some way, why don't you simply play with $\dot\theta_d\left(t\right)$ and then compute $\theta_d\left(t\right)$ accordingly by means of integration? As you might know, integration is a well posed operation compared with the derivative.

On the other hand, $\dot\theta\left(t\right)$ is a different "beast". In physical systems you barely have access to it (you'd need ad-hoc sensors for measuring it). Valid alternatives are: Kalman estimation, Savitzky-Golay filtering, use of proper D term in the controller equipped with a high-frequency pole.

Title: how to implement tracking problem with PID controller

Body:
I'm trying to implement the tracking problem for this example using PID controller. The dynamic equation is

$$
I  \ddot{\theta} + d \dot{\theta} + mgL \sin(\theta) = u
$$

where

$\theta$ : joint variable.

$u$ : joint torque

$m$ : mass.

$L$ : distance between centre mass and joint.

$d$ : viscous friction coefficient

$I$ : inertia seen at the rotation axis.

$\textbf{Regulation Problem:}$

In this problem, the desired angle $\theta_{d}$ is constant and $\theta(t)$ $\rightarrow \theta_{d}$ and $\dot{\theta}(t)$ $\rightarrow 0$ as $t$ $\rightarrow \infty$. For PID controller, the input $u$ is determined as follows

$$
u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \underbrace{0}_{\dot{\theta}_{d}} -  \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d} - \theta(\tau)) d\tau
$$

The result is

and this is my code  main.m

```<code>clear all
clc

global error;
error = 0;

t = 0:0.1:5;

x0 = [0; 0];

[t, x] = ode45('ODESolver', t, x0);

e = x(:,1) - (pi/2); % Error theta

plot(t, e, 'r', 'LineWidth', 2);
title('Regulation Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\theta_{d} - \theta(t)$', 'Interpreter','LaTex');
grid on
</code>```

and ODESolver.m is

```<code>function dx = ODESolver(t, x)

global error; % for PID controller

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5;
Kd = 1.9;
Ki = 0.02;

% u: joint torque
u = Kp*(pi/2 - x(1)) + Kd*(-x(2)) + Ki*error;
error = error + (pi/2 - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

end
</code>```

$\textbf{Tracking Problem:}$

Now I would like to implement the tracking problem in which the desired angle $\theta_{d}$ is not constant (i.e. $\theta_{d}(t)$); therefore, $\theta(t)$ $\rightarrow \theta_{d}(t)$ and $\dot{\theta}(t)$ $\rightarrow \dot{\theta}_{d}(t)$ as $t$ $\rightarrow \infty$. The input is

$$
u = K_{p} (\theta_{d} - \theta(t)) + K_{d}( \dot{\theta}_{d}(t) -  \dot{\theta}(t) ) + \int^{t}_{0} (\theta_{d}(t) - \theta(\tau)) d\tau
$$

Now I have two problems namely to compute $\dot{\theta}_{d}(t)$ sufficiently and how to read from txt file since the step size of ode45 is not fixed. For the first problem, if I use the naive approach which is

$$
\dot{f}(x) = \frac{f(x+h)-f(x)}{h}
$$

the error is getting bigger if the step size is not small enough. The second problem is that the desired trajectory is stored in txt file which means I have to read the data with fixed step size but I'v read about ode45 which its step size is not fixed. Any suggestions!

Edit:

For tracking problem, this is my code

main.m

```<code>clear all
clc

global error theta_d dt;
error = 0;

theta_d = load('trajectory.txt');

i    = 1;
t(i) = 0;
dt   = 0.1;
numel(theta_d)
while ( i < numel(theta_d) )
   i = i + 1;
   t(i) = t(i-1) + dt;
end

x0 = [0; 0];
options= odeset('Reltol',dt,'Stats','on');
[t, x] = ode45(@ODESolver, t, x0, options);

 e = x(:,1) - theta_d; % Error theta

plot(t, x(:,2), 'r', 'LineWidth', 2);
title('Tracking Problem','Interpreter','LaTex');
xlabel('time (sec)');
ylabel('$\dot{\theta}(t)$', 'Interpreter','LaTex');
grid on
</code>```

ODESolver.m

```<code>function dx = ODESolver(t, x)

persistent i theta_dPrev

if isempty(i)
    i = 1;
    theta_dPrev = 0;
end

global error theta_d dt ;

dx = zeros(2,1);

%Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 35.5;
Kd = 12.9;
Ki = 1.5;

if ( i == 49 )
    i = 48;
end
% theta_d first derivative
theta_dDot  = ( theta_d(i) - theta_dPrev ) / dt;
theta_dPrev = theta_d(i);

% u: joint torque
u = Kp*(theta_d(i) - x(1)) + Kd*( theta_dDot - x(2)) + Ki*error;
error = error + (theta_dDot - x(1));

dx(1) = x(2);
dx(2) = 1/I*(u - d*x(2) - m*g*L*sin(x(1)));

i = i + 1;
end
</code>```

trajectory's code is

```<code>clear all
clc

a = 0:0.1:(3*pi)/2;

file = fopen('trajectory.txt','w');

for i = 1:length(a)
    fprintf(file,'%4f \n',a(i));
end

fclose(file);
</code>```

The result of the velocity is

Is this correct approach to solve the tracking problem?


问:
Dock command does not seem to work
We bought a new Create2 robot and started using it. But when we issue the dock command the robot moves for a bit and does not go back to the base.

The base is not hidden or obstructed and the create2 is just a couple of feet away. we need help to figure out why it does not see the base.

Just to clarify that even using the DOCK button on the create2 does not make the create2 go back to the base

答:
The dock command and pressing the dock button on the robot enable the same behavior. The robot does not go directly to the dock, because it does not know where the dock is. Instead, it drives around until it happens to encounter the dock's infrared beams, which extend about 10 feet in front of the dock. Once Roomba encounters the dock beams, it will line up with the charging base and dock.

If you place the robot in front of the dock, facing it, and press the dock button, it should go directly onto the dock. If the robot starts somewhere else, it may take several minutes to find the dock. This is normal.

Title: Dock command does not seem to work


问:
Sending commands to Roomba from PC
I'm trying to send some commands to the Roomba. However is behaving strange.

This is the manual that I'm using.
http://www.irobot.com/~/media/MainSite/PDFs/About/STEM/Create/create_2_Open_Interface_Spec.pdf

First of all. I have consulted several manuals, some of them say that the default baudrate is 115200, however it works for me at 57200.

I'm trying to get a response from the Roomba sending the following comand

Examples:
• To turn on iRobot Create’s Play LED only:
128 132 139 2 0 0

However, the Roomba goes crazy and start going around. Any idea what's happening or what I'm not doing? Or what should I do first?

Thank you.

答: If your robot communicates at 57600 baud, then it is a Roomba 400 series robot, and you should use the manual that En3rGy posted, not the one for Create 2. That said, the LEDs command (139) is identical in all versions of Create and Roomba, so clearly something else is going on. Try a baud rate of 57600. It's possible that when you send data at 57200, some data corruption is happening as Create tries to interpret it at 57600 baud.</p>

Title: Sending commands to Roomba from PC


问:
Position and Object Data Tracking
For a class project, I'm working with a weight stack:

I'm trying to simultaneously measure:

the position of a moving weight stack

the value of the weight based on a calibrated/preloaded position in the stack, not via load sensor. (e.g. think a stack of plate weights where the sensor knows in advance that 1 plate = 10lbs, 2 plates = 20lbs, etc.)

The weight stack and the base camp chip/sensor/laser would be within two feet of the weight stack, so I don't need anything overly strong. My requirement is that it is small/unobtrusive and cost effective. I've looked into a few options, but I'm not an engineer so I'm not sure if I am on the right track.

How would you do this? Is there any research that I could check out?

答:
If there is a gap, divot, crease, etc, between plates you might be able to count the number of plates that go past using a tactile micro switch. The holes in the plates used to 'pick' the weight could be used if the plates interface were sufficiently smooth to prevent counting transitions. Of course you would need to know the direction of movement and be able to tell when the completely picked stack was passed. </p>

<p>Another alternative might be using a light beam through the pole the weights ride on, which would be blocked by the pin. Though this seems more error prone. </p>

Title: Position and Object Data Tracking

Body:
If there is a gap, divot, crease, etc, between plates you might be able to count the number of plates that go past using a tactile micro switch. The holes in the plates used to 'pick' the weight could be used if the plates interface were sufficiently smooth to prevent counting transitions. Of course you would need to know the direction of movement and be able to tell when the completely picked stack was passed.

Another alternative might be using a light beam through the pole the weights ride on, which would be blocked by the pin. Though this seems more error prone.


问:
Recommendation for good source of Robotic Components
Im looking for a good source for robotic components like sheel/tracked robot chasis, motors, sensors, communication and mechanics. I thought about using raspberry and arduino as platforms for automation, is that an good idea? Im asking as i dont know yet much about the motors/drives uses for powering robots.

Thanks!

Uli

答: Radioshack is good, although it recently went bankrupt, but a few of their stores are still around. Texas Instruments is a good source, it's out of Texas and has nearly any electronic components needed.</p>


问:
Cheapest 3D printer for gears?
It would be incredibly useful if I could print my own gearing solutions, even if I have to print the gears one at a time.  However, I do not know how the market's cheapest printers will accommodate this task.

--The gears need be 2-3 inches in diameter, and they will bear only a very light load (much less than 1 foot pound), so the material need not be strong or machinable.

--The tolerances need only be sufficient for the teeth to mate robustly, preventing any hang.  Unfortunately, I do not have a sense of what tolerances will allow gears to mate properly.

--(If the machine is precise enough to print a hole to statically mate with a shaft of a specified dimensions due to friction, excellent.  If not, I can probably improvise a tiny shaft hole with adhesive.)

--Because this may be used in close proximity to pavement, a melting temperature in excess of 100F is desirable but not required.

--Because any given element will interact kinetically only with other elements that have also been 3D printed (except a metallic shaft), compatibility with external resources is not required.

I would be grateful to anyone who could shed some light on this issue!

答:
The printrbot jr was voted the best quality printer for its price in Makezine: 3D printer guide. It is only around $300-500. I use the printrbot jr, and its quality is good most of the time. Since it is made like it is, it can't do well with large overhangs. It builds up layer by layer, so you have to orient it to build up without overhangs. To make a solid gear about 5 inches in diameter and with a height of about 3 inches would take around an hour, since it is difficult to fill solid, but there are settings to make it hollow or even honeycomb, which is sturdy and takes less time. The printrbot jr is also compared to a hot glue gun, since its filament is pushed out so close it spreads out, which makes small holes sometimes get filled in and nice smooth sides a little rough, but it is good with a quick nail file to it. Hope this helps.

BTW: if you order a printrbot jr, it comes in two ways, build your self and premade. The premade is an extra $100, but IT'S WORTH IT!!! The build yourself one is insanely difficult to put together and some pieces don't fit.

Title: Cheapest 3D printer for gears?


问:
Roomba Create 2 problem reading distance traveled
I am trying to work with the create2. In using the "get distance traveled" command (id 142) I am getting back incorrect data. My simple test case logic is

I am working with the Create2_TetheredDrive.py example
and adding this

```<code>    elif k == 'PLUS' or k == 'MINUS': # Move 200mm forward or backward
        # reset distance measurement by sending request
        sendCommandASCII('142 19')
        # ignore/discard the data returned
        recv_basic(connection)
        # set velocity mm/s
        v = 200;
        if k=='MINUS':
            v=-v
        # start moving
        cmd = struct.pack(">Bhh", 145, v, v)
        sendCommandRaw(cmd)
        # pause 1 second
        time.sleep(1);
        # stop moving
        cmd = struct.pack(">Bhh", 145, 0, 0)
        sendCommandRaw(cmd)
        # get distance traveled
        sendCommandASCII('142 19')
        data = recv_basic(connection)
        dist = struct.unpack('>h',data)
        print(dist)
</code>```

I consistently numbers near -25 for moving forward, and +25 for moving backward.
If I wait for 2 seconds, I get -50 for moving forward, and +50 for moving backward. The documentation says it should return the distance traveled in mm, so these numbers seem to be off by a factor of -8.

Anyone have any suggestions? Thanks.

p.s. I had to add this function to the example as well

```<code>def recv_basic(the_socket):
    the_socket.settimeout(0.1)
    total_data=[]
    while True:
        try:
            data = the_socket.recv(8192)
            total_data.append(data)
        except:
            break
    return ''.join(total_data)
</code>```

答:
There are two problems at work here.

First, you wrote a function to get data from the robot using sockets, but the robot is being connected to via a pySerial serial object. I wrote a function for you to use instead of recv_basic. I'll be sure to add this helper function and others like it to Create2_TetheredDrive.py when I can. Using this function, running your code gives me a result of around 185-190, which is about what I'd expect (given that the robot has to accelerate and decelerate during that one second; it doesn't immediately get up to speed).

```<code># get16Signed returns a 16-bit signed value.
# Whether it blocks is based on how the connection was set up.
def get16Signed():
    global connection

    try:
        return struct.unpack(">h", connection.read(2))[0]
    except serial.SerialException:
        print "Lost connection"
        tkMessageBox.showinfo('Uh-oh', "Lost connection to the robot!")
        connection = None
    except struct.error:
        print "Got unexpected data from serial port."
        return None
</code>```

Just add this function and replace your calls to recv_basic(connection) with get16Signed() (and also get rid of that call to struct.unpack as get16Signed will return an integer) and you should be all set!

Second, we found a bug with sensor packets which return values in units of millimeters in robot firmware versions prior to 3.3.0. The affected packets are '19' and '40.' Please see the relevant sections of the Open Interface Spec for more details.

Happy hacking!

Please note that while I'm an iRobot employee, the postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.

Body:
There are two problems at work here.

First, you wrote a function to get data from the robot using sockets, but the robot is being connected to via a pySerial serial object. I wrote a function for you to use instead of recv_basic. I'll be sure to add this helper function and others like it to Create2_TetheredDrive.py when I can. Using this function, running your code gives me a result of around 185-190, which is about what I'd expect (given that the robot has to accelerate and decelerate during that one second; it doesn't immediately get up to speed).

# get16Signed returns a 16-bit signed value.
# Whether it blocks is based on how the connection was set up.
def get16Signed():
    global connection

    try:
        return struct.unpack(">h", connection.read(2))[0]
    except serial.SerialException:
        print "Lost connection"
        tkMessageBox.showinfo('Uh-oh', "Lost connection to the robot!")
        connection = None
    except struct.error:
        print "Got unexpected data from serial port."
        return None

Just add this function and replace your calls to recv_basic(connection) with get16Signed() (and also get rid of that call to struct.unpack as get16Signed will return an integer) and you should be all set!

Second, we found a bug with sensor packets which return values in units of millimeters in robot firmware versions prior to 3.3.0. The affected packets are '19' and '40.' Please see the relevant sections of the Open Interface Spec for more details.

Happy hacking!

Please note that while I'm an iRobot employee, the postings on this site are my own and don't necessarily represent iRobot's positions, strategies, or opinions.

ContentLicense: CC BY-SA 4.0


问:
Determine current roomba state / operating mode
Using the SCI messages, I would like to determine the current operating mode or state of a iRobot Roomba 780. Finally, I would like to detect and separate four states:

Cleaning

In docking station

Returning to docking station

Error (e.g. trapped on obstacle)

What is a fast and reliable way to detect those states using SCI data?

The Roomba SCI Sensor packets "Remote Control Command" and "Buttons" seem to return the currently called commands and not the currently executed ones.

答:
I found a partial answers to my question:
The "current" sensor message indicates if the roomba is operating (< 0) or in the docking station (> 0).
Open remains error handling and "searching for docking station".
I'll adapt my answers if I get further.

EDIT 2015-05-05:
The solution which seems to work looks in pseudo code as follows:

```<code>if ( charging_state == 5 ) {
  return ERROR;
}
else if ( charging_state == 2 ) {
  return CHARGING;
}
else {
  if ( charger_available == 2 ) {
    return INDOCK;
  }
  else {
    if ( main_brush_current != 0 && current < 0 ) {
      return WORKING;
    }
    else {
      return ERROR;
    }
  }
}
</code>```

The "variables" within the if statements are corresponding sensor messages given in the document, linked by @Andrea.

The "return values" indicate the corresponding Roomba states.

Body:
I found a partial answers to my question:
The "current" sensor message indicates if the roomba is operating (< 0) or in the docking station (> 0).
Open remains error handling and "searching for docking station".
I'll adapt my answers if I get further.

EDIT 2015-05-05:
The solution which seems to work looks in pseudo code as follows:

if ( charging_state == 5 ) {
  return ERROR;
}
else if ( charging_state == 2 ) {
  return CHARGING;
}
else {
  if ( charger_available == 2 ) {
    return INDOCK;
  }
  else {
    if ( main_brush_current != 0 && current < 0 ) {
      return WORKING;
    }
    else {
      return ERROR;
    }
  }
}

The "variables" within the if statements are corresponding sensor messages given in the document, linked by @Andrea.

The "return values" indicate the corresponding Roomba states.


问:
How to calculate the center of mass Jacobian matrix of a robot arm
I have a 4-DOF robot arm system with 4 revolute joints arranged in an open-chain fashion like below:

Assume that each link’s mass is a point mass located at p_i and each link’s center of mass is at p_i.

What I am trying to do is calculate the center of mass Jacobian matrix of the arm.
I found some related materials online Center of Mass Jacobian.But I am still not very sure about how to calculate it. Could anybody give me some hint? Thanks!

答:
The method you mention from elysium-labs is perfectly functional. I try to clarify it a little bit and I give you some C++ like code.

Let me start by recapping on jacobian matrix itself: Jacobian matrix relates the joint rates to the linear and angular velocity of the end-effector (EE). In other words, jacobian expresses the contribution of each joint velocity to EE velocity. There are basically two ways of calculating jacobian: Analytically and Geometric. Analytical jacobian is partial derivatives while geometric jacobian is based on geometric interpretation of motion.

In theory, you can calculate CoM jacobian by doing partial derivative but this a very tedious task and impossible in practice. So geometric jacobian is the way to go. Lets see the formula for geometric jacobian (no CoM yet!):

The upper row shows the contribution of each joint to the end-effector velocity and bottom row for the angular velocity. $P_i$ and $z_i$ are position and orientation of the joint, extracted from Denavit Hartenberg parameters. '$\times$' is cross product operator.

```<code>Jacobian.block<3,1>(0,0)   = z0.cross(pEE_L-p0);
Jacobian.block<3,1>(0,1)   = z1.cross(pEE_L-p1);

Jacobian.block<3,1>(3,0)   = z0;
Jacobian.block<3,1>(3,1)   = z1;
</code>```

.block<3,1>(0,1) means matrix block of size 3x1 starting from element 0,1.

When calculating the CoM jacobian, we do not consider the end-effector. In other words, you must calculate the contribution of each joint movement to the velocity of the center of mass.

First, you need to calculate the partial CoM, i.e., the CoM of each kinematic chain w.r.t the base. For example, partial CoM if you consider only $l_1$, then only $l_1, l_2$, then $l_1, l_2, l_3$ and so on. The rational is that $\theta_1$ has a bigger contribution to CoM velocity comparing to $\theta_4$. Now that we have all the necessary information, we calculate the geometric CoM jacobian, similarly to the kinematic jacobian:

```<code>Jacobian_CoM.block<3,1>(0,0)  = (partial_coms(3,0)/total_mass) * (z0.cross (R0*(partial_coms.block<3,1>(0,0)-PB)-p0));
Jacobian_CoM.block<3,1>(0,1)  = (partial_coms(3,1)/total_mass) * (z1.cross (R0*(partial_coms.block<3,1>(0,1)-PB)-p1));
</code>```

In the above code, "Note that we had to rotate the partial COM to get it into the base links coordinate frame", hence $R_0$. Furthermore, "the resultant linear velocity should be scaled by the mass of the partial COM because the COM is the average of the multi-mass system and high velocities on smaller masses play a lesser role on the total velocity of the COM."

In my code, I was interested in linear velocity only and that's why CoM jacobian is $3\times n$.

A good source for understanding the concept of jacobian, its geometric interpretation, and the notation that I've used in this answer, is this set of slides.

Body:
The method you mention from elysium-labs is perfectly functional. I try to clarify it a little bit and I give you some C++ like code.

Let me start by recapping on jacobian matrix itself: Jacobian matrix relates the joint rates to the linear and angular velocity of the end-effector (EE). In other words, jacobian expresses the contribution of each joint velocity to EE velocity. There are basically two ways of calculating jacobian: Analytically and Geometric. Analytical jacobian is partial derivatives while geometric jacobian is based on geometric interpretation of motion.

In theory, you can calculate CoM jacobian by doing partial derivative but this a very tedious task and impossible in practice. So geometric jacobian is the way to go. Lets see the formula for geometric jacobian (no CoM yet!):

The upper row shows the contribution of each joint to the end-effector velocity and bottom row for the angular velocity. $P_i$ and $z_i$ are position and orientation of the joint, extracted from Denavit Hartenberg parameters. '$\times$' is cross product operator.

Jacobian.block<3,1>(0,0)   = z0.cross(pEE_L-p0);
Jacobian.block<3,1>(0,1)   = z1.cross(pEE_L-p1);

Jacobian.block<3,1>(3,0)   = z0;
Jacobian.block<3,1>(3,1)   = z1;

.block<3,1>(0,1) means matrix block of size 3x1 starting from element 0,1.

When calculating the CoM jacobian, we do not consider the end-effector. In other words, you must calculate the contribution of each joint movement to the velocity of the center of mass.

First, you need to calculate the partial CoM, i.e., the CoM of each kinematic chain w.r.t the base. For example, partial CoM if you consider only $l_1$, then only $l_1, l_2$, then $l_1, l_2, l_3$ and so on. The rational is that $\theta_1$ has a bigger contribution to CoM velocity comparing to $\theta_4$. Now that we have all the necessary information, we calculate the geometric CoM jacobian, similarly to the kinematic jacobian:

Jacobian_CoM.block<3,1>(0,0)  = (partial_coms(3,0)/total_mass) * (z0.cross (R0*(partial_coms.block<3,1>(0,0)-PB)-p0));
Jacobian_CoM.block<3,1>(0,1)  = (partial_coms(3,1)/total_mass) * (z1.cross (R0*(partial_coms.block<3,1>(0,1)-PB)-p1));

In the above code, "Note that we had to rotate the partial COM to get it into the base links coordinate frame", hence $R_0$. Furthermore, "the resultant linear velocity should be scaled by the mass of the partial COM because the COM is the average of the multi-mass system and high velocities on smaller masses play a lesser role on the total velocity of the COM."

In my code, I was interested in linear velocity only and that's why CoM jacobian is $3\times n$.

A good source for understanding the concept of jacobian, its geometric interpretation, and the notation that I've used in this answer, is this set of slides.


问:
Robotics with Kinect
I want to learn robotics and really interested in making a robot based on Kinect sensor.

I see so many projects like this one, and just wondering how it works on top level. I downloaded Kinect SDK and did some basic tutorials, but I just don't think that Microsoft SDK is the library to use for real robotics projects. Any suggestions where to start and what library to use? Any good books in particular or online resources?

答:
Welcome to the Robotics StackExchange! The Kinect is only a sensor, albeit a relatively powerful one for its price. To use it with a robot, you need to cover basically two bases:

Get a robot that is able to interface with it

That means you need to somehow build/buy something consisting of processing unit (a microcontroller or PC) with USB connectivity and the appropriate drivers to recognize and control the Kinect (usually easier with a PC running Linux). This thing also needs to be able to provide appropriate power input to the sensor.

You also want it to be able to move, so you need to add motors or other actuators to it and figure out how to get your processing unit to control them (usually easier with a microcontroller).

If you want it to be able to process all the data from the Kinect and make complex decisions by itself you will need enough processing power onboard (usually a PC). Otherwise, you might have to add wireless communication with other computers that will do the heavy calculations.

Figure out how to get the Kinect data and what to do with it

The Microsoft SDK is perfectly usable, as is OpenNI or libfreenect... It really comes down to what operating system is running on your robot. I am not aware of how to interface the Kinect with less powerful microcontrollers not running a popular OS (Windows/Linux/Mac) and while it may be possible it definitely is not desirable (as in it would be a lot of work for a beginner).

Once you've chosen which programming library and driver to use to get Kinect data, you will need to find out what you want to do with the data you get. The raw sensor data is just a range image, but some of these libraries can provide some higher-level info extracted from these range images such as skeletonized models of people.

In short:

Start by figuring out what kind of robot you want, how you want it to move, where the Kinect will be placed and so on.

Choose your processing unit after searching for alternatives that are able to interface with the Kinect easily. I would suggest something like a netbook running Linux to a beginner, with a microcontroller board like the Arduino connected via USB to control motors and other low-level components of the robot.

Once you have the basic components of your robot ready (e.g. mechanical parts in place, PC running Linux, motor control working, etc) add the Kinect to the mix by connecting it to your processing unit and start working on what to do with it.

Title: Robotics with Kinect


答:
You can treat your differential equation as a third order ODE, because of the integral in the PID controller. For this I will denote the integral of $\theta$ as $\Theta$, such that the ODE becomes,

$$
I\ddot{\theta}+d\dot{\theta}+mgL\sin(\theta)+K_p\left(\theta-\theta_d(t)\right)+K_i\left(\Theta-\int_0^t\theta_d(\tau)d\tau\right)+K_d\left(\dot{\theta}-\dot{\theta}_d(t)\right)=0
$$

One way you can write this into MATLAB code would be:

```<code>clear all
clc

% Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5000;
Ki = 166667;
Kd = 50;

% Define reference signal
syms t
a = 3 * pi / 10;
temp = a * t;
% temp = pi/2 + 0*t;
 r = matlabFunction(temp, 'Vars', t);
 R = matlabFunction(int(temp,t), 'Vars', t);
dr = matlabFunction(diff(temp), 'Vars', t);
clear temp t

fun = @(t,x) [x(2); x(3); -m*g*L/I*sin(x(2))-d/I*x(3)] - [0; 0; ...
    Kp / I * (x(2) -  r(t)) + ...
    Ki / I * (x(1) -  R(t)) + ...
    Kd / I * (x(3) - dr(t))];

T = linspace(0, 5, 1e3);
x0 = [0; 0; 0];
[t, Y] = ode45(@(t,x) fun(t,x), T, x0);

y = zeros(size(t));
for i = 1 : length(t)
    y(i) = r(t(i));
end

figure
    subplot(2,1,1)
    plot(t, y, t, Y(:,2))
    xlabel('Time [s]')
    ylabel('Position [rad]')
    subplot(2,1,2)
    plot(t, y-Y(:,2))
    xlabel('Time [s]')
    ylabel('Position error [rad]')
</code>```

Body:
You can treat your differential equation as a third order ODE, because of the integral in the PID controller. For this I will denote the integral of $\theta$ as $\Theta$, such that the ODE becomes,

$$
I\ddot{\theta}+d\dot{\theta}+mgL\sin(\theta)+K_p\left(\theta-\theta_d(t)\right)+K_i\left(\Theta-\int_0^t\theta_d(\tau)d\tau\right)+K_d\left(\dot{\theta}-\dot{\theta}_d(t)\right)=0
$$

One way you can write this into MATLAB code would be:

clear all
clc

% Parameters:
m = 0.5;       % mass (Kg)
d = 0.0023e-6; % viscous friction coefficient
L = 1;         % arm length (m)
I = 1/3*m*L^2; % inertia seen at the rotation axis. (Kg.m^2)
g = 9.81;      % acceleration due to gravity m/s^2

% PID tuning
Kp = 5000;
Ki = 166667;
Kd = 50;

% Define reference signal
syms t
a = 3 * pi / 10;
temp = a * t;
% temp = pi/2 + 0*t;
 r = matlabFunction(temp, 'Vars', t);
 R = matlabFunction(int(temp,t), 'Vars', t);
dr = matlabFunction(diff(temp), 'Vars', t);
clear temp t

fun = @(t,x) [x(2); x(3); -m*g*L/I*sin(x(2))-d/I*x(3)] - [0; 0; ...
    Kp / I * (x(2) -  r(t)) + ...
    Ki / I * (x(1) -  R(t)) + ...
    Kd / I * (x(3) - dr(t))];

T = linspace(0, 5, 1e3);
x0 = [0; 0; 0];
[t, Y] = ode45(@(t,x) fun(t,x), T, x0);

y = zeros(size(t));
for i = 1 : length(t)
    y(i) = r(t(i));
end

figure
    subplot(2,1,1)
    plot(t, y, t, Y(:,2))
    xlabel('Time [s]')
    ylabel('Position [rad]')
    subplot(2,1,2)
    plot(t, y-Y(:,2))
    xlabel('Time [s]')
    ylabel('Position error [rad]')


问:
What wireless technology to use to control robots in classroom?
I want to build a cheap robot programmable in Scratch graphical language, that could be employed during lessons in school. Scratch code is interpreted on a PC, so on the robot there should be only the code that receives specific commands (i.e. drive forward) and transmits sensors' measurements.

I'm looking for a wireless technology that will allow me to exchange information between robot and PC with at least 30Hz rate. It should also allow to work at least 16 robots simultaneously in the same room and have a range of at least 20m.

I did tests with BLuetooth, but sometimes there are connectivity issues, and pairing devices can be a hassle in a classroom. I have also tried WiFi modules, but pinging it showed average time of 19ms, but maximum of more than 500ms, so I'm afraid that it won't be able to control linefollower robot for example.

Can you point me to some other, preferably cheap (under 10$ per module) wireless technologies? Or maybe my worries about WiFi are exaggerated?

答:
Unfortunately I don't have enough rep to comment or I would do that instead of a full blown answer.

I think Rf modules would be the way to go in this situation, I have played around with this trans/recv pair before and had pretty good results, however I don't think the range would quite be enough for your setup.

With regards to you mentioning 16 robots communicating at the same time, I would suggest using some kind of "call sign" system, where each message you transmit could be pre-fixed with a letter, or a number to ensure the correct messages are sent to the correct robots and vice versa. If the message does not contain the robots unique call sign, discard message, if it does, process.

You could alternatively have each robot transmitting on a different frequency, however I don't know how well this would work, and you would have to get a variable frequency RF module.

Sounds interesting though!
Good luck


问:
Monocular vs. stereo computer vision robustness for object detection
Are there some genaral rules for the robustness between monocular and stereo vision when considering object detection? I am especially interested in the automotive field - considering distance/obstacle/car detection (see video links below).

Someone told me monocular vision is more robust than stereo. I guess this may be true if the monocular algorithm is well written (and especially verified over lots of input data)... but once you input (image) data that has not been verified it may probably provide unexpected results, right? With stereo vision one does not really care about the contents of the image as long as texture/lighting conditions allow stereo matching and the object detection is then done within the point cloud.

I consider following usage:

Monocular

Stereo

The monocular sample video seems to have sometimes problems detecting the cars in front (the bounding boxes disappear once in a while). The stereo sample seems to be more robust - the car in front clearly is detected in all of sequnce image frames.

答:
Two videos that you provided are not doing the same task. The stereo system is just measuring distance of different points in space (which happens to include a car in that video). It will show any object in front of it but won't classify the object. So, cars or people or trash cans mean the same thing to that algorithm and it will just return the distances.

On the other hand the monocular system is doing object classification into two categories of "Car" and "Not Car". Another part of this algorithm is detecting characteristic lines of a driving lane (two blue lines and the yellow line). The distance of each car is calculated based on relative position of of the car to these detected lines.

In conclusion, if you want to do object (cars) detection, you need to use a method similar to what was illustrated in monocular system video. If you only want distance, stereo vision is one of many available methods.


问:
How to tune a PIV controller?

How could I tune the above PIV controller? I am trying to get the system to have a settling time of < 1 second, P.O < 15% and zero steady state error.

答:
It may prove useful to generate the closed loop transfer function with the $K$, $C_{1}$, $C_{2}$ and $Gm$ terms.

$$\frac{C(s)}{R(s)} = \frac{G(s)}{1+G(s)H(s)}$$

It may look something like;

$$\frac{Gm(C_{1}+K)}{1+Gm(C_{1}+C_{2})}$$ initially

You may also want to convert the Gm transfer function into two 2nd order equations of the form;

$$\frac{(As - B)}{(s^2 + 2\zeta_1\omega_{n1} s + \omega_{n1} ^2) (s^2 + 2\zeta_2 \omega_{n2} s + \omega_{n2} ^2)}$$

by finding the roots of the $Gm$ denominator.
You could think about creating the partial fractions of this.
You will then know the natural frequencies $(\omega_{n1} , \omega_{n2} )$ and damping factors $(\zeta_1, \zeta_2)$ of the plant $Gm$. This will give you an idea if the plant is stable and the iteration rate needed for your PI(V) control.

The closed loop transfer function above will give you an idea that with $(C_{1}+K)$ and $(C_{1}+C_{2})$ you may like to think about setting $K_p$ negative and greater than the feed forward term $K$, until you get an under damped response. Then adjust the $K_i$ and $K_v$ terms until the required rise time and overshoot are achieved.

$K_p=-190$, $K_i=-19$, and $K_v=-3.5$ will give a fast response but play around for the one you want.

Title: How to tune a PIV controller?


答:
There is a product from Pololu called the Wixel. In a classroom environment, when one of them transmits a signal all of the others will receive the message. The radio interconnections between the wixels are essentially just like serial communications.

It wouldn't be too much work to bash together a protocol that can either 'broadcast' to all devices in the room, or 'target' a specific device by including its id in the message. You'd have to program the Wixel's so that when it receives a non-broadcast message that isn't coded to its own ID, it simply ignores the message.

They are around $20 each or less if bought in bulk.

In fact with a setup like this, the wixels could actually pair up if you want one controller per bot. Even with short bursts of comms there is a possibility of data collision when two signals go out simultaneously.  In those situations you could look into how such issues are handled in other technologies, such as ethernet, for example.

If you go this route, you might also want to investigate the I2C protocol which also allows numerous devices to communicate all on a common bus. The main difference there though would be that they have a CLK sync line which they can all monitor in order to decide if its their turn to talk or not. There will be no sync line to use over the air. That's where data collisions come in to play. The I2C protocol is really simple and can be learned pretty quickly. I think the same concepts could be applied.

You can also tell the Wixel to talk on a different channel. Up to 128 apparently, so you could have a fairly decent number of paired devices.

Title: What wireless technology to use to control robots in classroom?

Body:
I want to build a cheap robot programmable in Scratch graphical language, that could be employed during lessons in school. Scratch code is interpreted on a PC, so on the robot there should be only the code that receives specific commands (i.e. drive forward) and transmits sensors' measurements.

I'm looking for a wireless technology that will allow me to exchange information between robot and PC with at least 30Hz rate. It should also allow to work at least 16 robots simultaneously in the same room and have a range of at least 20m.

I did tests with BLuetooth, but sometimes there are connectivity issues, and pairing devices can be a hassle in a classroom. I have also tried WiFi modules, but pinging it showed average time of 19ms, but maximum of more than 500ms, so I'm afraid that it won't be able to control linefollower robot for example.

Can you point me to some other, preferably cheap (under 10$ per module) wireless technologies? Or maybe my worries about WiFi are exaggerated?


问:
Calculating acceleration and velocity
I'm writing some Quad Copter software and beginning to implement an altitude hold mode.

To enable me to do this I need to get an accurate reading for vertical velocity. I plan to use a Kalman filter for this but first I need to ensure that I'm getting the correct velocity from each individual sensor.

I have done this but I'm not 100% sure its correct so I was hoping to get some confirmation on here.

My first sensor is a Lidar distance sensor, I calculated acceleration and velocity using the following code:

```<code>float LidarLitePwm::getDisplacement()
{
    int currentAltitude = read();
    float displacement = currentAltitude - _oldAltitude;
    _oldAltitude = currentAltitude;

    return displacement; //cm
}

//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s

float lidarDisplacement = _lidar->getDisplacement();
_currentLidarVelocity = lidarDisplacement / time;
</code>```

The second sensor is an accelerometer. I calculated acceleration and velocity using the following code:

```<code>Imu::Acceleration Imu::getAcceleration()
{
    //Get quaternion
    float q[4];
    _freeImu.getQ(q);

    //Get raw data
    float values[9];
    _freeImu.getValues(values);

    //Extract accelerometer data
    float acc[3];
    acc[0]= values[0]; //x
    acc[1]= values[1]; //y
    acc[2]= values[2]; //z

    //Gravity compensate
    _freeImu.gravityCompensateAcc(acc, q);

    //Convert acceleration from G to cm/s/s
    _acceleration.x = acc[0] * 9.8 * 100;
    _acceleration.y = acc[1] * 9.8 * 100;
    _acceleration.z = acc[1] * 9.8 * 100;

    return _acceleration; //cm/s/s
}

//Time since last update
float time = (1.0 / ((float)FLIGHT_CONTROLLER_FREQUENCY / 10.00)); // 50Hz, 0.02s

//Get accel
Imu::Acceleration imuAcceleration = _imu->getAcceleration();

//Get velocity
currentZVelocity += imuAcceleration.z * time; //cm/s
</code>```

It would be great if someone could confirm if this is correct (or not)

Thanks
Joe

答:
I noticed a weird thing in your code -- It looks like you are calculating a first derivative of something labeled altitude in LidarLitePwm::getAcceleration() (which would be velocity, and not acceleration) and later integrating it to get something labeled velocity and (which would in fact be altitude).

So either your variable and method names are wrong, or you are missing one more derivative in LidarLitePwm::getAcceleration(). Also it is a litle weird that you are using the time when integrating acceleration and not when differentiating it. It should be the same in both directions. If you include the time in seconds, your units will be cm/s/s as stated in the comments, if you don't they will be cm/tick/tick.

Since it seems that you want to reach velocity as the final product anyway, I would skip calculating the accelerations, rename LidarLitePwm::getAcceleration() to LidarLitePwm::getVelocity() and change it to something like this:

```<code>float LidarLitePwm::getVelocity(float time)
{
    int currentAltitude = read();
    float velocity = (currentAltitude - _oldAltitude) / time;
    _oldAltitude = currentAltitude;

    return velocity; //cm/s/s
}
</code>```

... all this assuming that the read() function returns distance from ground in cm.

The accelerometer stuff seems ok.

Btw if you are holding altitude with PID don't you need the altitude rather than velocity?

Title: Calculating acceleration and velocity

Body:
I noticed a weird thing in your code -- It looks like you are calculating a first derivative of something labeled altitude in LidarLitePwm::getAcceleration() (which would be velocity, and not acceleration) and later integrating it to get something labeled velocity and (which would in fact be altitude).

So either your variable and method names are wrong, or you are missing one more derivative in LidarLitePwm::getAcceleration(). Also it is a litle weird that you are using the time when integrating acceleration and not when differentiating it. It should be the same in both directions. If you include the time in seconds, your units will be cm/s/s as stated in the comments, if you don't they will be cm/tick/tick.

Since it seems that you want to reach velocity as the final product anyway, I would skip calculating the accelerations, rename LidarLitePwm::getAcceleration() to LidarLitePwm::getVelocity() and change it to something like this:

float LidarLitePwm::getVelocity(float time)
{
    int currentAltitude = read();
    float velocity = (currentAltitude - _oldAltitude) / time;
    _oldAltitude = currentAltitude;

    return velocity; //cm/s/s
}

... all this assuming that the read() function returns distance from ground in cm.

The accelerometer stuff seems ok.

Btw if you are holding altitude with PID don't you need the altitude rather than velocity?


问:
Controlling the iRobot Create 2 with MATLAB
I teach a university sophomore level MATLAB programming class for engineers, and I am planning on using the create2 for their final project. There is a nice simulator and MATLAB toolbox for the Create, but the toolbox utilizes some of the commands that no longer exist on the Create 2, thus it doesn't work correctly. And of course is doesn't support any of the newer commands. In addition, I want to be able to "cut the cord" so I am using a Raspberry Pi on the Create to pipe data to the serial port, and TCPIP sockets to send the data from a remote computer running MATLAB to the Pi/Create. If anyone is working on a similar configuration, I'd love to trade notes and share the pain.</p>

答:
The API hasn't changed that much.  An overview of the changes can be found on the last page of the OI Spec.  I am confused as to what might be causing the toolbox to break.  Can you elaborate?

Using a RasPI to pipe serial data over TCP/IP to a desktop sounds like an expensive and error-prone way to "cut the cord".  Using a serial to bluetooth or XBee module will be much easier.


问:
RC Transmitter Quadcopter with Arduino
I have a WL v262 quadcopter and I want to control it using Arduino instead of the joysticks on the transmitter. I opened up the transmitter and saw that each joystick has 2 potentiometers on the PCB and that the voltage for each pot goes from 0-3.3V. I used arduino's PWM and a low pass filter and connected the output of the filtered output to the potentiometer's analog pin which is connected to the PCB (I cannot desolder and take out the pots from the PCB) but even with this $V_{out}$ going onto the analog pin, my transmitter's display gave ????

Now I am really confused and frustrated because I don't know how else to control this transmitter other than attaching stepper motors to the joysticks and manually controlling the transmitter but this is really my last resort. Can someone help me with this? I have spent hours and hours trial and error but I am getting nowhere.

Here is the PCB of the transmitter:

答: I have the answer because I´ve the same problem, you can use a PWM but need use a RC low pass filter in the output http://www.avdweb.nl/arduino/hardware-interfacing/super-simple-dac.html</p>

Title: RC Transmitter Quadcopter with Arduino


问:
Interfacing high-resolution image sensors with ARM Board
I'm working on a project requiring HD (Stereo) Video Processing. Most of High Resolution (5MP+) Sensors use MIPI-CSI interface.

I managed to get a board with an Exynos5 SoC. The SoC Itself has 2 MIPI-CSI2 interfaces, the problem is that the pins to those interfaces are not exposed and It's (almost) impossible to reach them. So I decided to use the USB3.0 Buses.

The problem is when I get to Significant bandwidth (~5.36 Gibibits/s per sensor), I don't think USB3.0 will work out. Bandwidth = Colordepth*ColorChannels*PixelCount*FPS but this could be solved with a Compressed stream (via a coprocessor)

I was thinking that Cypress' CYUSB306X chip was a good candidate for the job, but one of the problems is that I can't do BGA Soldering by hand nor have been able to find a BGA Soldering Service in Switzerland.

Any Ideas on other interfaces I could implement or other coprocessors with MIPI-CSI2 Interface?

Just a final remark, space and weight are important as this is supposed to be mounted on a drone.

答:
you dont need to do the soldering for the cypress processor, there are always sdk boards like http://www.cypress.com/?rID=58321

or really just get something like http://www.e-consystems.com/UltraHD-USB-Camera.asp

browse e-consystems site and you will find something usefull


问:
RGB-D SLAM - Compute Information Matrix
currently im working on a RGB-D SLAM with a Kinect v1 Camera. In the front-end the SLAM estimates the pose with Ransac as an initial guess for the ICP. With the pose estimation i transform the pointcloud to a pointcloud-scene which represents my map.

To smooth the map im trying to implement a graph optimizing algorithm (g2o).
Until now, there is no graph representation in my frontend, so i started to integrate that.

Im trying to build a .g2o file with the following fromat:

VERTEX_SE3 i x y z qx qy qz qw

where x, y, z is the translation and qx, qy, qz, qw ist the Rotation in respect to the initial coordinate system. And,

EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx, qy, qz, qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66

Translation and rotation for the edge is the pose estimate that i compute with Ransac and ICP (visual odometry).

Now im getting stuck with the information matrix.
I read the chapter 3.4 THE INFORMATION FILTER in Thrun's Probabolistic Robotics and several threads in this forum, such as:

The relationship between point cloud maps and graph maps

and

information filter instead of kalman filter approach

From the second link, i got this here.

The covariance update
  $$P_{+} = (I-KH)P$$
  can be expanded by the definition of K to be

$$ P_{+} = P - KHP$$
  $$ P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$

Now apply the matrix inversion lemma, and we have:

$$P_{+} = P - PH^T (HPH^T+R)^{-1} HP$$
  $$ P_{+} = (P^{-1} + H^TR^{-1}H)^{-1}$$

Which implies:
  $$ P_{+}^{-1} = P^{-1} + H^TR^{-1}H$$

The term $P^{-1}$ is called the prior information,$$H^TR^{-1}H$$
  is the sensor information (inverse of sensor variance), and this gives us  $P^{-1}_+$, which is the posterior information.

Could you please point this out for me.
What data do i need to compute the information matrix?

答:
From the wiki of the g2o file format:

The information matrix or precision matrix which represent the
  uncertainty of   the measurement error is the inverse of the
  covariance matrix. Hence, it is symmetric and positive semi-definite.

Unfortunately there is no mentioning of the representation for the covariance matrix for that file format, that I could find. My guess is that it is a compact representation, so either a $6\times6$ matrix using $[x, y, z, \psi, \phi, \theta]$ (euler angles) or $[x, y, z, r_1, r_2, r_3]$ (scaled axis of rotation).

Title: RGB-D SLAM - Compute Information Matrix


问:
Jacobian transpose: How to calculate orientation error?
I'm confused about how to compute the error in orientation. All the documents I've read don't explain how to do it.

The error in position is simply the difference between the points.

Let's assume we have the orientation along the effector axis, and we represent the rotation with quaternions. I have two questions:

Is describing the orientation with quaternions a good approach?

How can we compute the error in orientation with the quaternions to use this in jacobian transpose?

答: The math involved in determining orientation error is described in this thread: Jacobian-based trajectory following</p>


问:
Raspberry Pi quadcopter thrashes at high speeds
I am attempting to build a Raspberry Pi based quadcopter. So far I have succeeded in interfacing with all the hardware, and I have written a PID controller that is fairly stable at low throttle. The problem is that at higher throttle the quadcopter starts thrashing and jerking. I have not even been able to get it off the ground yet, all my testing has been done on a test bench. I have ruled out bad sensors by testing each sensor individually, and they seem to be giving correct values. Is this a problem with my code, or perhaps a bad motor? Any suggestions are greatly appreciated.

Here is my code so far:

QuadServer.java:

```<code>package com.zachary.quadserver;

import java.net.*;
import java.io.*;
import java.util.*;

import se.hirt.pi.adafruit.pwm.PWMDevice;
import se.hirt.pi.adafruit.pwm.PWMDevice.PWMChannel;

public class QuadServer {
    private static Sensor sensor = new Sensor();

    private final static int FREQUENCY = 490;

    private static double PX = 0;
    private static double PY = 0;

    private static double IX = 0;
    private static double IY = 0;

    private static double DX = 0;
    private static double DY = 0;

    private static double kP = 1.3;
    private static double kI = 2;
    private static double kD = 0;

    private static long time = System.currentTimeMillis();

    private static double last_errorX = 0;
    private static double last_errorY = 0;

    private static double outputX;
    private static double outputY;

    private static int val[] = new int[4];

    private static int throttle;

    static double setpointX = 0;
    static double setpointY = 0;

    static long receivedTime = System.currentTimeMillis();

    public static void main(String[] args) throws IOException, NullPointerException {

        PWMDevice device = new PWMDevice();
        device.setPWMFreqency(FREQUENCY);

        PWMChannel BR = device.getChannel(12);
        PWMChannel TR = device.getChannel(13);
        PWMChannel TL = device.getChannel(14);
        PWMChannel BL = device.getChannel(15);

        DatagramSocket serverSocket = new DatagramSocket(8080);

        Thread read = new Thread(){
                public void run(){
                    while(true) {
                    try {
                            byte receiveData[] = new byte[1024];
                            DatagramPacket receivePacket = new DatagramPacket(receiveData, receiveData.length);
                            serverSocket.receive(receivePacket);
                            String message = new String(receivePacket.getData());
                            throttle = (int)(Integer.parseInt((message.split("\\s+")[4]))*12.96)+733;
                            setpointX = Integer.parseInt((message.split("\\s+")[3]))-50;
                            setpointY = Integer.parseInt((message.split("\\s+")[3]))-50;

                        receivedTime = System.currentTimeMillis();

                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }
                }
        };
        read.start();

        while(true)
        {
            Arrays.fill(val, calculatePulseWidth((double)throttle/1000, FREQUENCY));

            double errorX = -sensor.readGyro(0)-setpointX;
            double errorY = sensor.readGyro(1)-setpointY;

            double dt = (double)(System.currentTimeMillis()-time)/1000;

            double accelX = sensor.readAccel(0);
            double accelY = sensor.readAccel(1);
            double accelZ = sensor.readAccel(2);

            double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
            double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));

            double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
            double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

            if(dt > 0.01)
            {

                PX = errorX;
                PY = errorY;

                IX += errorX*dt;
                IY += errorY*dt;

                IX = 0.95*IX+0.05*accelAngleX;
                IY = 0.95*IY+0.05*accelAngleY;

                DX = (errorX - last_errorX)/dt;
                DY = (errorY - last_errorY)/dt;

                outputX = kP*PX+kI*IX+kD*DX;
                outputY = kP*PY+kI*IY+kD*DY;
                time = System.currentTimeMillis();
            }

            System.out.println(setpointX);

            add(-outputX+outputY, 0);
            add(-outputX-outputY, 1);
            add(outputX-outputY, 2);
            add(outputX+outputY, 3);

            //System.out.println(val[0]+", "+val[1]+", "+val[2]+", "+val[3]);
            if(System.currentTimeMillis()-receivedTime < 1000)
            {
                BR.setPWM(0, val[0]);
                TR.setPWM(0, val[1]);
                TL.setPWM(0, val[2]);
                BL.setPWM(0, val[3]);
            } else
            {
                BR.setPWM(0, 1471);
                TR.setPWM(0, 1471);
                TL.setPWM(0, 1471);
                BL.setPWM(0, 1471);
            }

        }
    }

    private static void add(double value, int i)
    {
        value = calculatePulseWidth(value/1000, FREQUENCY);
        if(val[i]+value > 1471 && val[i]+value < 4071)
        {
            val[i] += value;
        }else if(val[i]+value < 1471)
        {
            //System.out.println("low");
            val[i] = 1471;
        }else if(val[i]+value > 4071)
        {
            //System.out.println("low");
            val[i] = 4071;
        }
    }

    private static int calculatePulseWidth(double millis, int frequency) {
        return (int) (Math.round(4096 * millis * frequency/1000));
    }
}
</code>```

Sensor.java:

```<code>package com.zachary.quadserver;

import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;
import com.pi4j.io.gpio.GpioController;
import com.pi4j.io.gpio.GpioFactory;
import com.pi4j.io.gpio.GpioPinDigitalOutput;
import com.pi4j.io.gpio.PinState;
import com.pi4j.io.gpio.RaspiPin;
import com.pi4j.io.i2c.*;

import java.net.*;
import java.io.*;

public class Sensor {
    static I2CDevice sensor;
    static I2CBus bus;
    static byte[] accelData, gyroData;
    static long accelCalib[] = new long[3];
    static long gyroCalib[] = new long[3];

    static double gyroX = 0;
    static double gyroY = 0;
    static double gyroZ = 0;

    static double accelX;
    static double accelY;
    static double accelZ;

    static double angleX;
    static double angleY;
    static double angleZ;

    public Sensor() {
        //System.out.println("Hello, Raspberry Pi!");
        try {
            bus = I2CFactory.getInstance(I2CBus.BUS_1);

            sensor = bus.getDevice(0x68);

            sensor.write(0x6B, (byte) 0x0);
            sensor.write(0x6C, (byte) 0x0);
            System.out.println("Calibrating...");

            calibrate();

            Thread sensors = new Thread(){
                    public void run(){
                        try {
                            readSensors();
                        } catch (IOException e) {
                        System.out.println(e.getMessage());
                    }
                    }
            };
            sensors.start();
        } catch (IOException e) {
            System.out.println(e.getMessage());
        }
    }

    private static void readSensors() throws IOException {
        long time = System.currentTimeMillis();
        long sendTime = System.currentTimeMillis();

        while (true) {
            accelData = new byte[6];
            gyroData = new byte[6];
            int r = sensor.read(0x3B, accelData, 0, 6);
            accelX = (((accelData[0] << 8)+accelData[1]-accelCalib[0])/16384.0)*9.8;
            accelY = (((accelData[2] << 8)+accelData[3]-accelCalib[1])/16384.0)*9.8;
            accelZ = ((((accelData[4] << 8)+accelData[5]-accelCalib[2])/16384.0)*9.8)+9.8;
            accelZ = 9.8-Math.abs(accelZ-9.8);

            double hypotX = Math.sqrt(Math.pow(accelX, 2)+Math.pow(accelZ, 2));
            double hypotY = Math.sqrt(Math.pow(accelY, 2)+Math.pow(accelZ, 2));

            double accelAngleX = Math.toDegrees(Math.asin(accelY/hypotY));
            double accelAngleY = Math.toDegrees(Math.asin(accelX/hypotX));

            //System.out.println((int)gyroX+", "+(int)gyroY);

            //System.out.println("accelX: " + accelX+" accelY: " + accelY+" accelZ: " + accelZ);

            r = sensor.read(0x43, gyroData, 0, 6);
            if(System.currentTimeMillis()-time >= 5)
            {
                gyroX = (((gyroData[0] << 8)+gyroData[1]-gyroCalib[0])/131.0);
                gyroY = (((gyroData[2] << 8)+gyroData[3]-gyroCalib[1])/131.0);
                gyroZ = (((gyroData[4] << 8)+gyroData[5]-gyroCalib[2])/131.0);

                angleX += gyroX*(System.currentTimeMillis()-time)/1000;
                angleY += gyroY*(System.currentTimeMillis()-time)/1000;
                angleZ += gyroZ;

                angleX = 0.95*angleX + 0.05*accelAngleX;
                angleY = 0.95*angleY + 0.05*accelAngleY;

                time = System.currentTimeMillis();
            }
            //System.out.println((int)angleX+", "+(int)angleY);
            //System.out.println((int)accelAngleX+", "+(int)accelAngleY);
        }
    }

    public static void calibrate() throws IOException {
        int i;
        for(i = 0; i < 3000; i++)
        {
            accelData = new byte[6];
            gyroData = new byte[6];
            int r = sensor.read(0x3B, accelData, 0, 6);
            accelCalib[0] += (accelData[0] << 8)+accelData[1];
            accelCalib[1] += (accelData[2] << 8)+accelData[3];
            accelCalib[2] += (accelData[4] << 8)+accelData[5];

            r = sensor.read(0x43, gyroData, 0, 6);
            gyroCalib[0] += (gyroData[0] << 8)+gyroData[1];
            gyroCalib[1] += (gyroData[2] << 8)+gyroData[3];
            gyroCalib[2] += (gyroData[4] << 8)+gyroData[5];
            try {
                Thread.sleep(1);
            } catch (Exception e){
                e.printStackTrace();
            }
        }
        gyroCalib[0] /= i;
        gyroCalib[1] /= i;
        gyroCalib[2] /= i;

        accelCalib[0] /= i;
        accelCalib[1] /= i;
        accelCalib[2] /= i;
        System.out.println(gyroCalib[0]+", "+gyroCalib[1]+", "+gyroCalib[2]);
    }

    public double readAngle(int axis)
    {
        switch (axis)
        {
            case 0:
                return angleX;
            case 1:
                return angleY;
            case 2:
                return angleZ;
        }

        return 0;
    }

    public double readGyro(int axis)
    {
        switch (axis)
        {
            case 0:
                return gyroX;
            case 1:
                return gyroY;
            case 2:
                return gyroZ;
        }

        return 0;
    }

    public double readAccel(int axis)
    {
        switch (axis)
        {
            case 0:
                return accelX;
            case 1:
                return accelY;
            case 2:
                return accelZ;
        }

        return 0;
    }
}
</code>```

Edit:

I have re-written my code in C++ to see if it will run faster but it's still running at about the same speed(about 15 ms per cycle or about 66 Hz).

This is my new code in C++:

```<code>#include <wiringPi.h>
#include <wiringPiI2C.h>

#include <sys/socket.h>
#include <netinet/in.h>

#include <string.h>
#include <string>
#include <iostream>
#include <unistd.h>

#include <boost/thread.hpp>

#include <time.h>

#include <cmath>

#define axisX 0
#define axisY 1
#define axisZ 2

#define kP 20
#define kI 0
#define kD 0

#define FREQUENCY 490

#define MODE1 0x00
#define MODE2 0x01
#define SUBADR1 0x02
#define SUBADR2 0x03
#define SUBADR13 0x04
#define PRESCALE 0xFE
#define LED0_ON_L 0x06
#define LED0_ON_H 0x07
#define LED0_OFF_L 0x08
#define LED0_OFF_H 0x09
#define ALL_LED_ON_L 0xFA
#define ALL_LED_ON_H 0xFB
#define ALL_LED_OFF_L 0xFC
#define ALL_LED_OFF_H 0xFD

// Bits
#define RESTART 0x80
#define SLEEP 0x10
#define ALLCALL 0x01
#define INVRT 0x10
#define OUTDRV 0x04

#define BILLION 1000000000L

using namespace std;

double accelCalX = 0;
double accelCalY = 0;
double accelCalZ = 0;

double gyroCalX = 0;
double gyroCalY = 0;
double gyroCalZ = 0;

double PX;
double PY;

double IX = 0;
double IY = 0;

double DX;
double DY;

double lastErrorX;
double lastErrorY;

int throttle = 1471;

int sensor = wiringPiI2CSetup(0x68);
int pwm = wiringPiI2CSetup(0x40);

array<int,4> motorVal;

struct timespec now, then;

int toSigned(int unsignedVal)
{
    int signedVal = unsignedVal;
    if(unsignedVal > 32768)
    {
        signedVal = -(32768-(unsignedVal-32768));
    }
    return signedVal;
}

double getAccel(int axis)
{
    double X = (toSigned((wiringPiI2CReadReg8(sensor, 0x3B) << 8)+wiringPiI2CReadReg8(sensor, 0x3C)))/1671.8;
    double Y = (toSigned((wiringPiI2CReadReg8(sensor, 0x3D) << 8)+wiringPiI2CReadReg8(sensor, 0x3E)))/1671.8;
    double Z = (toSigned((wiringPiI2CReadReg8(sensor, 0x3F) << 8)+wiringPiI2CReadReg8(sensor, 0x40)))/1671.8;

    X -= accelCalX;
    Y -= accelCalY;
    Z -= accelCalZ;
    Z = 9.8-abs(Z-9.8);

    switch(axis)
    {
        case axisX:
            return X;
        case axisY:
            return Y;
        case axisZ:
            return Z;
    }
}

double getGyro(int axis)
{
    double X = (toSigned((wiringPiI2CReadReg8(sensor, 0x43) << 8)+wiringPiI2CReadReg8(sensor, 0x44)))/1671.8;
    double Y = (toSigned((wiringPiI2CReadReg8(sensor, 0x45) << 8)+wiringPiI2CReadReg8(sensor, 0x46)))/1671.8;
    double Z = (toSigned((wiringPiI2CReadReg8(sensor, 0x47) << 8)+wiringPiI2CReadReg8(sensor, 0x48)))/1671.8;

    X -= gyroCalX;
    Y -= gyroCalY;
    Z -= gyroCalZ;

    switch(axis)
    {
        case axisX:
            return X;
        case axisY:
            return Y;
        case axisZ:
            return Z;
    }
}

void calibrate()
{
    int i;
    for(i = 0; i < 1500; i++)
    {
        accelCalX += (toSigned((wiringPiI2CReadReg8(sensor, 0x3B) << 8)+wiringPiI2CReadReg8(sensor, 0x3C)))/1671.8;
        accelCalY += (toSigned((wiringPiI2CReadReg8(sensor, 0x3D) << 8)+wiringPiI2CReadReg8(sensor, 0x3E)))/1671.8;
        accelCalZ += (toSigned((wiringPiI2CReadReg8(sensor, 0x3F) << 8)+wiringPiI2CReadReg8(sensor, 0x40)))/1671.8;

        gyroCalX += (toSigned((wiringPiI2CReadReg8(sensor, 0x43) << 8)+wiringPiI2CReadReg8(sensor, 0x44)))/1671.8;
        gyroCalX += (toSigned((wiringPiI2CReadReg8(sensor, 0x45) << 8)+wiringPiI2CReadReg8(sensor, 0x46)))/1671.8;
        gyroCalX += (toSigned((wiringPiI2CReadReg8(sensor, 0x45) << 8)+wiringPiI2CReadReg8(sensor, 0x46)))/1671.8;
        usleep(1000);
    }

    accelCalX /= i;
    accelCalY /= i;
    accelCalZ /= i;
    accelCalZ -= 9.8;

    gyroCalX /= i;
    gyroCalY /= i;
    gyroCalZ /= i;

    cout << accelCalX << " " << accelCalY << " " << accelCalZ << "\n";
}

int calculatePulseWidth(double millis, int frequency) {
    return (int)(floor(4096 * millis * frequency/1000));
}

void add(double value, int i)
{
    value = calculatePulseWidth(value/1000, FREQUENCY);
    if(motorVal[i]+value > 1471 && motorVal[i]+value < 4071)
    {
        motorVal[i] += value;
    }else if(motorVal[i]+value < 1471)
    {
        //System.out.println("low");
        motorVal[i] = 1471;
    }else if(motorVal[i]+value > 4071)
    {
        //System.out.println("low");
        motorVal[i] = 4071;
    }
}

void getThrottle()
{
    int sockfd,n;
    struct sockaddr_in servaddr,cliaddr;
    socklen_t len;
    char mesg[1000];

    sockfd=socket(AF_INET,SOCK_DGRAM,0);

    bzero(&servaddr,sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(8080);
    bind(sockfd,(struct sockaddr *)&servaddr,sizeof(servaddr));

    while(true)
    {
        len = sizeof(cliaddr);
        n = recvfrom(sockfd,mesg,1000,0,(struct sockaddr *)&cliaddr,&len);
        mesg[n] = 0;
        string message(mesg);
        string values[5];

        int valIndex = 0;
        int lastIndex = 0;
        for(int i = 0; i < message.length(); i++)
        {
            if(message[i] == ' ')
            {
                values[valIndex] = message.substr(lastIndex+1, i);
                lastIndex = i;
                valIndex++;
            }
        }
        values[valIndex] = message.substr(lastIndex+1, message.length());

        throttle = calculatePulseWidth(((stoi(values[4])*12.96)+733)/1000, FREQUENCY);
    }
}

void setAllPWM(int on, int off) {
    wiringPiI2CWriteReg8(pwm, ALL_LED_ON_L, (on & 0xFF));
    wiringPiI2CWriteReg8(pwm, ALL_LED_ON_H, (on >> 8));
    wiringPiI2CWriteReg8(pwm, ALL_LED_OFF_L, (off & 0xFF));
    wiringPiI2CWriteReg8(pwm, ALL_LED_OFF_H, (off >> 8));
}

void setPWM(int on, int off, int channel) {
    wiringPiI2CWriteReg8(pwm, LED0_ON_L + 4 * channel, (on & 0xFF));
    wiringPiI2CWriteReg8(pwm, LED0_ON_H + 4 * channel, (on >> 8));
    wiringPiI2CWriteReg8(pwm, LED0_OFF_L + 4 * channel, (off & 0xFF));
    wiringPiI2CWriteReg8(pwm, LED0_OFF_H + 4 * channel, (off >> 8));
}

void setPWMFrequency(double frequency) {
    double prescaleval = 25000000.0;
    prescaleval /= 4096.0;
    prescaleval /= frequency;
    prescaleval -= 1.0;
    double prescale = floor(prescaleval + 0.5);
    int oldmode = wiringPiI2CReadReg8(pwm, MODE1);
    int newmode = (oldmode & 0x7F) | 0x10;
    wiringPiI2CWriteReg8(pwm, MODE1, newmode);
    wiringPiI2CWriteReg8(pwm, PRESCALE, (floor(prescale)));
    wiringPiI2CWriteReg8(pwm, MODE1, oldmode);
    usleep(50000);
    wiringPiI2CWriteReg8(pwm, MODE1, (oldmode | 0x80));
}

void initSensor()
{
    wiringPiI2CWriteReg8(sensor, 0x6B, 0x0);
    wiringPiI2CWriteReg8(sensor, 0x6C, 0x0);
}

void initPWM()
{
    setAllPWM(0, 0);
    wiringPiI2CWriteReg8(pwm, MODE2, OUTDRV);
    wiringPiI2CWriteReg8(pwm, MODE1, ALLCALL);
    usleep(50000);
    int mode1 = wiringPiI2CReadReg8(pwm, MODE1);
    mode1 = mode1 & ~SLEEP;
    wiringPiI2CWriteReg8(pwm, MODE1, mode1);
    usleep(50000);
    setPWMFrequency(FREQUENCY);
}

double millis(timespec time)
{
    return (time.tv_sec*1000)+(time.tv_nsec/1.0e6);
}

double intpow( double base, int exponent )
{
    int i;
    double out = base;
    for( i=1 ; i < exponent ; i++ )
    {
        out *= base;
    }
    return out;
}

int main (void)
{

    initSensor();
    initPWM();

    cout << "Calibrating..." << "\n";
    calibrate();

    boost::thread server(getThrottle);

    clock_gettime(CLOCK_MONOTONIC, &then);

    while(true)
    {
        motorVal.fill(throttle);

        clock_gettime(CLOCK_MONOTONIC, &now);

        double dt = (millis(now)-millis(then))/1000;

        then = now;

        double accelX = getAccel(0);
        double accelY = getAccel(1);
        double accelZ = getAccel(2);

        double hypotX = sqrt(intpow(accelX, 2)+intpow(accelZ, 2));
        double hypotY = sqrt(intpow(accelY, 2)+intpow(accelZ, 2));

        double accelAngleX = (180/3.14)*(asin(accelY/hypotY));
        double accelAngleY = (180/3.14)*(asin(accelX/hypotX));

        double errorX = -getGyro(0);
        double errorY = getGyro(1);

        PX = errorX;
        PY = errorY;

        IX += errorX*dt;
        IY += errorY*dt;

        IX = 0.95*IX+0.05*accelAngleX;
        IY = 0.95*IY+0.05*accelAngleY;

        DX = (errorX-lastErrorX)*dt;
        DY = (errorY-lastErrorY)*dt;

        lastErrorX = errorX;
        lastErrorY = errorY;

        double outputX = kP*PX+kI*IX+kD*DX;
        double outputY = kP*PY+kI*IY+kD*DY;

        add(outputY, 0);//-outputX+
        add(outputY, 1);//-outputX-
        add(outputY, 2);//outputX-
        add(outputY, 3);//outputX+

        setPWM(0, motorVal[0], 12);
        setPWM(0, motorVal[1], 13);
        setPWM(0, motorVal[2], 14);
        setPWM(0, motorVal[3], 15);
    }
}
</code>```

In addition two of the motors seem like they are lagging when I turn the quadcopter fast in one direction. Also for some strange reason the quadcopter seems less responsive to P gain; I have it at 20 in the C++ version and it is working about the same as when I had it at 1.5 in the java version.

Edit:

After doing some more testing I have determined that reading from the MPU6050 and writing to the PCA9685 board that I am using to control the ESCs is the source of the delay. Does anybody know how to speed this up?

Edit:

I managed to speed up my code to about 200 Hz by changing the i2c baud rate, but the quadcopter is still thrashing. I have spent hours trying to tune the pid controller, but it doesn't seem to help at all.

答: So, I figured out that the problem was vibration. I first tried using a running average to smooth the data that I was getting from my MPU, but the delay that it created made it oscillate a lot, so I tried a low pass filter instead, which worked GREAT! Thank you to everyone for your help!</p>