答:
I recommend

Nvidia TK1

Odroid XU3

Both have GPIO that you can program. And both have a desktop version Linux (or Android, in Odroid XU3).

Odroid XU3 can set display video size in boot script. I don't know about TK1.

Your point 4) can be achieved by using VNC. Basically you can use your laptop to view the display from your board. Here is a tutorial for setting VNC:
VNC Tutorial

I strongly recommend VNC.

Title: I need help with choosing a computer on the board

Body:
I need a computer on the board like raspberry pi for vending machine (I want to replace the original controller). This is list of some requirements:

1) It should have pins to connect to the mdb protocol & other stuff through gpio.

2) Good performance. There will be a display with browser showing rails application running. I've tried a raspberry pi B+, but it's too slow (it can't even run a browser with speed like a laptop pc). So, I want to choose a more powerful system like odroid, wandboard etc.

3) Custom video output. Sometimes I need to display FullHD(1920x1080), sometimes I need to show at 768x1024 (yes, the computer should simply rotate video output)

4) I don't want to connect microcomputer to display directly, not through HDMI, DVI or something like this. (This is not required, but very desirable).

Please help me choose. Nowadays I try to choose from odroid, wandboard or pandaboard. Are there any other computers? What version of the computer is advisable?


问:
What is the cheapest way to make led sensitive to sound around?
There is always a way to do this using arduino, rasberry pi etc.  However, in many cases in discussion in forums i've come across things where whole 'logic' can be uploaded to $0.50 chip. Instead of $50 dollar part. DRASTIC change. This defines a line between a one time thing that you made as a hobby, and something you can sell around. </p>

<p>So basically if i want led to get brightest at loud sound and almost off on silence.
Or with button that switch to 100% on all the time. </p>

答:
If the intention is only to have one LED glowing based on surrounding sound, use a condenser micro phone amplifier connect the output to LED, If you are doing some robotic project and already audio out put is available, you can connect LED directly with a voltage divider to avoid over voltage, also you can have an Op-Amp circuit to drive the LED.The solution will be based on  where are you trying to use this? how do you capture the surrounding audio? what kind of surrounding audio are you expecting?

EDITED: based on your comment,

You need a pre-amplifier to capture audio from mic, refer somthing similar to this circuit https://electronics.stackexchange.com/questions/71335/how-to-amplify-electret-microphone-to-range-from-0-5v-with-bias-of-2-5v

After this convert Audio to attractive light pattern, for that you can use db level indicator something similar to circuit in following link.
http://bestengineeringprojects.com/electronics-projects/a-versatile-decibel-meter-audio-level-indicator-spectrum-analyser/ (google for LM3914 circuits or same can be done with series of transistors and diodes, which works based on forward bias threshold voltage)

You can also combine this with rotating patterns of leds whose speed is based on audio input, if you google you will find some circuit. I can give some hint, it is possible with 555 and CD4017.

ContentLicense: CC BY-SA 3.0


问:
Converting a linear acceleration command into a DC motor command?
I'm constructing a 2 wheels balancing robot which uses a PID controller.  I've tuned my parameters on numerical simulations based on a continuous inverted pendulum system so that the simulated inverted pendulum balances by controlling the horizontal (linear) cart acceleration $\ddot{x}$.

Now that I've done this, I want to take the next step and turn my PID control commands into electrical commands onto a DC motor to give the desired linear acceleration $\ddot{x}$. However I'm not sure how exactly to do this for my specific robot's motors.  Are there experimental tests should I run to determine how to convert PID commands into DC motor acceleration commands? Or is there a formula to do this based on the motor's specifications?

Update

The non-linear dynamic equation I'm using is

$$L\ddot{\theta}=gsin(\theta)+\ddot{x}(t)cos(\theta)+Ld(t)$$

where $\ddot{x}(t)$ is the linear acceleration, $g$ is the acceleration due to gravity, and $\ddot{\theta}$ is the angular acceleration, and $d(t)$ is an external disturbance to the system. To simplify things, I've linearized the equations around $\theta\approx0$, yielding

$$L\ddot{\theta}=g\theta+\ddot{x}(t)+Ld(t)$$

I've assumed that the only control input is the cart's linear acceleration $\ddot{x}(t)$, and chose this control command as $\ddot{x}(t)=K_1\theta(t) + K_2\int_0^t\theta(t) dt + K_3\dot{\theta}$, where $K_i$ are the PID gains.

答:
First comment, the equation you gave for your PID seems a bit weird. The PID normally acts to steer an error, let's say $e_{\theta}=\theta_d-\theta$, to zero, where $\theta_d\left(t\right)$ is your set-point, possibly varying, and $\theta\left(t\right)$ is the current feedback.

If we always assume $\theta_d=0$ - is this your task? -, then you have to consider the right sign for the feedback, i.e. $-\theta$, since usually the PID gains are deemed to be positive. This way though, you'll prevent yourself from having fun while trying to stabilize the pole in non-upright positions (i.e. $\theta_d \neq 0$, wherein the linearization does not hold).

What is missing is to make the dynamic equation of linear motion explicit, that is something like $\ddot{x}=f\left(x,u,d_x,t\right)$, where $u$ is the motor torque and $d_x$ are other type of disturbances (e.g. friction forces) acting on the system. You could plug this second equation in the first one coming up with a new PID whose output is directly $u$ and no longer $\ddot{x}$, changing thus the input to the system.

You might describe the function $f\left(\cdot\right)$ in terms of what you know about the system (e.g. wheels geometry to transform torque into force, motors' parameters to deal with transformation of voltage/current into mechanical torque), providing therefore a feed-forward term to help the PID do its job or even let the PID work alone in the closed-loop.

Another possibility is to replace the single PID scheme with a cascade of a inner loop where you control $\ddot{x}$ through the first PID (you have somehow to measure/estimate the linear acceleration), and an outer loop that finally controls the angular position of the pole by means of a second PID. Anyway, two PIDs means more gains to fine tune. You may also want to identify $f$ via proper identification techniques.

As a side note, one effective way to deal with all the disturbances and uncertainties you have in the real system that are not taken into account in the model is to resort to sliding mode control.

Finally, I'd really suggest you to search in literature: there are plenty of methods and models out of the box for you to read and use for such a classical control problem.


问:
Is it possible for a robot to navigate through predefined coordinates?
I am a total newbie in robotics so please bare with me.

I have a school project where my team has to design a robot that is capable of picking up 3 golf balls in different sizes at predefined locations. Then it has to drop these balls into their respective holes.

We are using an arduino chip in our robot.

I thought I could perhaps define a path for the robot, an invisible virtual path you may call. So imagining the platform as Cartesian plane, can I tell the robot go to where I want it to go? For example, go to (5,12)

Or do I need some sort of sensors so the robot figures it out by itself. Thanks for your time!

答:
So, this is my first answer here. Hopefully this will help!

The golf balls, given that they are present in predefined locations, seem to be either be on a graph/grid, or at certain distances away from each other on the same plane.

Firstly, for a grid. You need to have line sensors to detect the presence of a grid which is either a junction (basically a complete black line detected by all sensors) or a right-angled turn. In either case the trick is to assign co-ordinates to it and travelling based on these. For example to travel from (0,0) to (0,1) you need to go north by 1 co-ordinate. So starting from (0,0) the bot must follow the line north till it detects a junction. And so on for any co-ordinate.

This link should help:
http://www.robotix.in/tutorials/category/auto/grid

Secondly, for no grid. Assuming that these golf balls are at certain distances from the starting point, it is possible to create a virtual grid of your own where each co-ordinate is at fixed distances, just like on a grid or graph. In that case, the bot has to follow the same procedures to reach a destination but just by moving for those fixed distances. So it is suggested that your bot has encoders for the dc motors that will help in travelling precisely the distance that you wish to travel.

Good Luck!! \m/


答:
I was working on a similar project a while ago. Assuming that you are using an AUX for audio input, the best thing that I have done that seemed to work was using a TIP-31c NPN Transistor. You could also use a TIP-31a/b transistor.

Here's an image on how you could connect the transistor and get the affect you are looking for: http://cdn.instructables.com/F7J/0GZT/HH2VYB6L/F7J0GZTHH2VYB6L.LARGE.gif

Summarizing the image: You want to connect the Base of the transistor to the left or right channel of the AUX, LEDs to the collector and ground the Emitter.

Title: What is the cheapest way to make led sensitive to sound around?

Body:
There is always a way to do this using arduino, rasberry pi etc.  However, in many cases in discussion in forums i've come across things where whole 'logic' can be uploaded to $0.50 chip. Instead of $50 dollar part. DRASTIC change. This defines a line between a one time thing that you made as a hobby, and something you can sell around. </p>

<p>So basically if i want led to get brightest at loud sound and almost off on silence.
Or with button that switch to 100% on all the time. </p>


问:
How feasible is the idea of operating a robotic arm in a non-sophisticated way?
I and my team have to design a robot using an arduino chip. The objective of the robot is to grab golf balls at a set of golf pins at different heights and pre-defined locations. We couldn't figure out a possible mechanism that could collect the balls and drop them into the trailer except for a robot arm. However, we don't have experience and time in designing a sophisticated system for the arm like recognizing where the ball is and then grabbing it accordingly. What would a feasible option be compared to a non-sophisticated  robot arm?

Note:The robot must be autonomous.

答:
Possible options,

Use image processing, with opencv you can detect object make the arm to hold the object

If you are very good in electronics, make this ball to emit RF signal, the strength of signal will increase as we go close to ball, use this info to track ball. You can refer radio transmitter and receiver circuits.


问:
Quadcopter degrees of freedom
It might be kind of a stupid question but how many degrees of freedom are there in a typical quadcopter? I say some saying 4 and some saying 6. The difference stands in translation throughout the other 2 axis (horizontal ones). Being strict to what you can directly tell the quadcopter to do, only 4 movements are possible since you cannot apply a pure lateral force. But you can tilt to start a lateral movement and align the body right after and let it hover in a horizontal axis, theoretically. So, formally, how many degrees of freedom should I consider to exist?</p>

答: You have 6 degrees of freedom. Three translational and three rotational. You have linear movements along the xyz axis and rotations around the xyz axis.</p>

Title: Quadcopter degrees of freedom


问:
How to stabilize a quadcopter
Today was my quadcopter's first "flight". I'm running megapirate on a Crius AIOP v2 with a Turnigy Talon v2 frame.

I only touched the throttle stick on my remote, nothing else. When I felt the quadcopter was about to take off, I pushed the throttle just a little bit more, and the quadcopter oscillated 2 or 3 times and the just flipped over, landing on the propellers.

So, I broke 2 props, my frame feels a bit loose, I'll probably have to tighten the screws (I hope...). How can I tune the software so it will stabilize nicely after take off?

Edit :

I don't know if it was true oscillation or just random air flows making it unstable. I made some more tests yesterday and it was quite OK (even if I crashed a few times). This time, it was really oscillating but it was quite windy outside and the quadcopter managed to stabilize after all. So i'll probably have to tune my PIDs and find a way to do it without crashing.

Edit 2 : After some PID tuning, I managed to stabilize my quadcopter pretty well but it's still oscillating just a little bit. I guess I'll have to slightly change the values to get a perfect stabilization.

答: I would suggest that you build a rig that constricts the quadcopter to rotating along just one axis. Either roll or pitch. Then you need to tune the roll/pitch controller independently. I would suggest using zeigler nichols to tune the PIDs. Once you tune roll/pitch you can move onto yaw </p>

Title: How to stabilize a quadcopter


问:
Can a quadcopter hover while tilted?
Is there a way to make a quadcopter maintain steady hovering (no lateral movement, constant altitude) while tilted left or right?  If so, how can i accomplish this? </p>

答:
No. Once you're quad is tilted you are projecting the thrust of the motors at an angle which is not parallel to the z axis causing your quadcopter to move linearly along the x-y plane.

If you must hover while tilted, you can use stepper motors to keep the thrust projecting parallel to the z axis.

That would be an unusual quad though.

edit: spelling

Title: Can a quadcopter hover while tilted?

ContentLicense: CC BY-SA 4.0


答:
The platform

When I hear

Easy to do

Quick and dirty

I always think of a Cartesian robot or a three axis XYZ mechanism.

But why would you want to use this instead of a more human robot arm ?

Multiple reasons :

Way easier to control : Cartesian is somehow more intuitive that cylindrical coordinate systems. You will not have has much math involved and the development time is going to be that quicker.

Easier to make.

Very effective. Cartesian robots were once the norm and they still are widely use, there is a reason for that

Using an arduino to control a Cartesian robot is probably going to be more straightforward and easy to make than a robot arm.

In case you still want to use an arm, you should start checking out Denavit-Hartenberg parameters. Here is a pretty good explanation about them. You can still apply it to a Cartesian robot if you just want to expand your knowledge and understand this. After all, it's only a convention.

Now that we have a likely platform lets dive into your problem.

You want to pick up balls and put them in a trailer. Since you don't have to do any recognition work (you know all the balls locations) it's one thing off of your shoulders. You only want the robot to go to precise locations, on after the other and do its job.

Thus the positions should be hard coded. Or you could simple give a file with different position as an argument.

Then, depending on what is the support for the balls (a shelve ? just lying on the floor on the floor ?) you could either grab them with a clip (sorry for giving you an image there : Google translate told me the word is a clip but I'm unsure that's the good word) or, push them of a table to fall into a trailer or use a suction cup... It's time to get creative ;) !

[EDIT already]

I just though about something else that you could do if the balls are on the floor. Why not a mobile base that rolls around and pick up the balls ? That's another feasible cheap possibility. And there is plenty of documentation on this :).

I saw your other question as well so I'm going to comment the answer there to give you my guess on how to do the actual movement from one point to another.

Title: How feasible is the idea of operating a robotic arm in a non-sophisticated way?

Body:
I and my team have to design a robot using an arduino chip. The objective of the robot is to grab golf balls at a set of golf pins at different heights and pre-defined locations. We couldn't figure out a possible mechanism that could collect the balls and drop them into the trailer except for a robot arm. However, we don't have experience and time in designing a sophisticated system for the arm like recognizing where the ball is and then grabbing it accordingly. What would a feasible option be compared to a non-sophisticated  robot arm?

Note:The robot must be autonomous.


答:
@Darth Hodorus answers is great ! But I just like to add some info about the second option.

I would definitely go for it and use dead-reckoning. It's easy to do if you got the encoders and you have plenty of tutorial on it =).

You'll realize that the major problem of D-R is drift. So you might have to think of a procedure to calibrate the robot each time. For example, with a Cartesian robot it's as easy as going to a locked position (a position where the robot can't move anymore i.e min in X, Y and Z) and define it as the (0,0,0) position. Indeed, you don't need to measure this position as you can mechanically find it.

Title: Is it possible for a robot to navigate through predefined coordinates?

Body:
I am a total newbie in robotics so please bare with me.

I have a school project where my team has to design a robot that is capable of picking up 3 golf balls in different sizes at predefined locations. Then it has to drop these balls into their respective holes.

We are using an arduino chip in our robot.

I thought I could perhaps define a path for the robot, an invisible virtual path you may call. So imagining the platform as Cartesian plane, can I tell the robot go to where I want it to go? For example, go to (5,12)

Or do I need some sort of sensors so the robot figures it out by itself. Thanks for your time!


答: Can you use something like a leaf rake shape to just pick up all the balls, or even a clam shell of leaf rake type 'hands' once you have the general location? </p>


问:
BeagleBone - PRU questions
I will be using at least one programmable real-time unit (PRU) to send pulses to a stepper motor driver but before I begin, I am trying to lay out the structure of my programs.

I am using this library PRU Linux API
for loading assembly code into the PRU instruction memory but there doesn't seem to be much documentation other then whats at that wiki and the source:
github-pru-packageh

My c program will be calculating the position of the sun using an algorithm and executing the assembly/writing a pulse count to the PRU(s) data memory so they can just switch on/off a gpio at my desired frequency and for the number of pulses required to turn a stepper the appropriate number of steps. I am not even sure if this is an acceptable method but I am pretty new at this and it seems like a simple way to accomplish my task

My Questions regarding the library functions are:

Is there a significant performance difference between using prussdrv_map_prumem or prussdrv_pru_write_memory to give the PRU(s) access to the pulse count?

Would it be better halt the PRU assembly program after has completed the tasks for each pulse count then re-execute it with new values, or keep the PRU program running and poll for a new pulse count to be written in?

I plan to send a pulse count every 10 seconds or so.

Any suggestions on revisiting the whole structure and logic are welcome as well.

答:
I am not familiar with PRU or the PRU Linux API, however, I can give you some general advice.

Is there a significant performance difference between using prussdrv_map_prumem or prussdrv_pru_write_memory to give the PRU(s) access to the pulse count?

Most likely yes. If I understood correctly, prussdrv_map_prumem gives you direct access to the PRU memory while prussdrv_pru_write_memory not only does that through a function call, but also has to decode the function arguments at least to know which memory in particular you are writing to.

To know the performance difference, you can put your write operations in an infinite loop, and then for example each 1000 times toggle a GPIO pin. Measure the difference in toggle times with an oscilloscope to see how long 1000 writes took (note that this includes the constant for loop, counter increment, if and gpio write as well). This should give you some measure of which method is faster and by how much.

But you should note two things. First, if you are controlling once every 10 seconds, performance is very likely not an issue for you. Second, you should be aware of any peculiarities with writing to the PRU memory. It is possible (again, I don't know PRU and I'm talking in general) that certain actions need to be taken in a certain order for the write to happen correctly. If you call the library-provided write function, you can be sure they took care of those peculiarities. If you plan on direct memory access, read the datasheet to know if there are any peculiarities and what they are. You can also look at the source code of the prussdrv_pru_write_memory function if available.

Would it be better halt the PRU assembly program after has completed the tasks for each pulse count then re-execute it with new values, or keep the PRU program running and poll for a new pulse count to be written in?

If you are planning on doing something useful every 10 seconds, then it's very very energy inefficient to poll instead of halt. Whenever you can halt and be sure somebody will wake you up, by all means do so.

Title: BeagleBone - PRU questions


问:
Minimising lateral drift in a PID (Arduino) controlled quadcopter using a 6DOF IMU
I'm developing a stabilisation system for an 'off-the-shelf' quadcopter by using an Arduino Mega and an IMU. The IMU is reading the angle of the quad, calculating motor commands by using a PID controller and applying them to the motors. It works well when constrained in a test bed, however in reality, although the quad is straight and level, it's drifting to one side because of its recent motor commands correcting the pitch/yaw. Is there any way I can (without using a vision system) keep the quad in one place without drifting?

I've looked into obtaining velocity by integrating the acceleration value, however it's extremely noisy and doesn't give a meaningful reading.

答:
Unfortunately, with just an IMU there's virtually no way for your quad to know that it's drifting so it can't stop it.

For outdoor flight you can add a GPS to detect the drift. For indoor flight, many people use vision systems to detect the drift. Depending on how close you are to walls, you could also look at ultrasonic range sensors to detect drift.


问:
Self-locking actuator: Friction versus worm gear
I am planning to control my bicycle derailleur with an electronic circuit, the advantages being multiple but not topic of this question.

The actuation would be performed by placing the assembly very close to the derailleur (but not on it) and by winding the usual steel cable around a spool placed on axis of a gear, and by using a motor to turn the gear.

This question concerns the alternatives for the spool self-locking mechanism (and eventually the kind of motor to use).

In the literature I found http://nabilt.github.io/diy_eshift/ and other similar ones that directly modify the derailleur with a stepper motor, and then are forced to keep the stepper motor powered up all the time to keep some torque on the shaft. I consider this inefficient, therefore I require a self-locking system to be able to remove power.

I come up with two ideas:

worm gear operated by a DC motor, where the steel cable is wound around the gear. This system is self-locking or almost self-locking, according to the gear ratio: the gear cannot (easily) drive the worm.

a motor driving normal gears with an appropriate reduction factor, but with an additional friction element, whose friction force is greater than the strength of the spring mounted on the derailleur (sorry if I mixed the technical terms). This is what normal bicycles already have: the friction along the cable and in the element placed n the handle is high and keeps the derailleur in place.

Both system would be assisted by a position-sensitive element (a trimmer?) to detect the actual position of the gear and/or of the derailleur, all configured in a closed feedback loop.

I don't consider additional options for the gear such as this one: http://www.agma.org/assets/uploads/publications/10FTM17_Kapelevich.pdf that consists of parallel-axis gears, whose teeth are however shaped in a manner to achieve self-locking without the need of a low-efficiency worm system.

From my point of view, I cannot see any clear advantage of worm gear vs friction except for:

the worm gear may allow me to build a more compact assembly, thanks to the two axes being perpendicular

speed vs torque: worm gears reduce the torque requirements, but the motor has to spin a lot and I cannot wait 3 seconds for each gear change.

Concerning the choice of the motor type (this is not the main question though), I think that:

a worm gear allows me to easily use a DC motor, since torque requirements are low and I don't need to detect the position of the shaft. Moreover, DC motors increase torque with decreasing speed, while stepper motors have the maximum torque defined by the stepping frequency.

DC are more compact and cheaper, important if I decide to offer this assembly as a kit and not unique, personal prototype.

I am working with 5V supply and I fear most easy-to get DC motors (old printers, scrap electronics) work on 12V, with a significant reduction of the available torque when operated at 5V.

I was looking for a "mechanics" section in Stack Exchange but I couldn't find it, so I opted for Robotics instead of Electronics.

答:
I suggest the worm gear. In our projects we use worm gear for having reliable locking characteristics without draining the batteries. If your only concern is the operation time, you can simply add one more gear to reduce the torque and increase the speed. In that case, you can use the benefits of a reliable locking system and faster operation. It will come with the expense of having a bigger motor and battery though. Keep in mind that vibrations can have a very bad effect on your friction-based lock.

To detect actual position of your derailleur, I suggest using a rotary encoder. I don't think you need to have a second feedback sensor on your gear and it will just overcomplicated the problem.


答:
IMUs have accumulating error and can not be a reliable sensors by themselves if you want to measure velocity or even worse, position.

I believe your safest bet would be doing a sensor fusion between an IMU and a vision sensor using feature extraction and Kalman filter. Using only a camera can introduce unpredictable errors specially in featureless environments.

The best resources would be finding monocular SLAM(Simultaneous Localization And Mapping) papers.

Title: Minimising lateral drift in a PID (Arduino) controlled quadcopter using a 6DOF IMU

Body:
I'm developing a stabilisation system for an 'off-the-shelf' quadcopter by using an Arduino Mega and an IMU. The IMU is reading the angle of the quad, calculating motor commands by using a PID controller and applying them to the motors. It works well when constrained in a test bed, however in reality, although the quad is straight and level, it's drifting to one side because of its recent motor commands correcting the pitch/yaw. Is there any way I can (without using a vision system) keep the quad in one place without drifting?

I've looked into obtaining velocity by integrating the acceleration value, however it's extremely noisy and doesn't give a meaningful reading.


问:
How many methods can I use to acquire depth data?
I am a newbie in Robotics. As far as I know, there're generally two ways to acquire depth information of a scene. One is the stereo vision method which applies two cameras. The other is the RGBD sensors such as Kinect and Primesense. It seems that both methods are in use currently. However, I do not know what are their advantages against each other.

I think the Kinect is a perfect solution over stereo vision ignoring its expense. So I have two questions here:

Is there advantages of binocular methods over Kinect besides expense?

As I know, both the two methods are confined with a limited distance detection range. In real world applications, we sometimes also need depth data at distance. Is there a method that we can acquire or estimate data information at far distance( not considering  laser-based detectors ).

Further more, my application may be a small flight vehicle. Which method and equipment should I choose? Will the traditional binocular camera be too slow for my application?

答:
I'm not familiar with internals of Kinect, but don't these RGBD devices essentially use stereo vision internally?

Regardless, stereo vision needs some software to give you RGBD values (there are most likely libraries already available for free), but those devices have that software already written for their particular technology. In the end, the data you get is similar.

Regarding your second question, the farther the distance between the cameras, the more 3D you can see. So if you go with two cameras, the advantage is that you can always adjust their distance based on how far your target is. Xtion, Kinect and similar technologies are optimized for a very particular scenario (recognizing people in front of the TV withing a couple of meters), so of course they are only suitable if your conditions are similar.


答:
Actually kinect sensors are similar to stereo-cameras. They are made up of two devices, an IR emitter and a IR receiver. The emitter projects a light-dots pattern of known shape and size, while the other device receives the actual reflected dots. From the deformation of the pattern is possible to find out depth, but the maths behind it, as far as I know, is the same of stereo triangulation with known correspondences (see this video as an example).

The main disadvantages of kinect when compared to stereo-camera systems are:

it does not work outdoors

it has a range of a few meters (less than 8m).

Stereo-cameras, on the contrary, work outdoors. Their range depends on the baseline, i.e. the distance between the cameras: the higher it is, the farther are the points that can be triangulated. However, most of commercial stereo-cameras, with baselines of few cm, have an accuracy of a few meters (see for example this commercial stereo device).
You can build yourself a stereo-rig to match the accuracy you need, but if you want good accuracy the device will not be cheap if compared to a kinect (depends mostly on the cameras you think to buy) and will cost you time to set up.
Finally, if you have to triangulate very far points, then you probably need to use a Structure from Motion algorithm (see Wikipedia for a quick definition).

Title: How many methods can I use to acquire depth data?

Body:
I am a newbie in Robotics. As far as I know, there're generally two ways to acquire depth information of a scene. One is the stereo vision method which applies two cameras. The other is the RGBD sensors such as Kinect and Primesense. It seems that both methods are in use currently. However, I do not know what are their advantages against each other.

I think the Kinect is a perfect solution over stereo vision ignoring its expense. So I have two questions here:

Is there advantages of binocular methods over Kinect besides expense?

As I know, both the two methods are confined with a limited distance detection range. In real world applications, we sometimes also need depth data at distance. Is there a method that we can acquire or estimate data information at far distance( not considering  laser-based detectors ).

Further more, my application may be a small flight vehicle. Which method and equipment should I choose? Will the traditional binocular camera be too slow for my application?


问:
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.

答:
Rasbian on Raspberry pi is not real time OS, when u try to read some value due to some processing factors there is a chance that values are read late and thus the overall real-time control does not occur which is very crucial for a quadcopter project. So it would be better to connect the mpu6050 and pwm out to a microcontroller rather than using Pi. You can use real time OS on Raspberry pi like FreeRTOS and ChibiOS/RT. Or instead attach a real-time hardware to serial port to make it real-time.

But for your case the trashing maybe caused due to incorrect PID values. Maybe you for got to put a -ve sign somewhere in the code. Please check this article clearly to know the correct -ve sign placements in code : How to build your own Quadcopter's Flight Controller.

But from my experience I would suggest you to trash Raspberry Pi for Quadcopter project since it might get heavy once u use a separate real-time unit, microcontroller and GPIO extensions for using more PWM pins. So instead of adding all those hardwares I would prefer you to use BeagleBone Back since it has an inbuilt Programmable real-time units that is you can use the 1GHz BBB processor for control logic and also its 2 independent PRU's to read PPM values from RC receiver and send PWM values to ESC's and it also has enough GPIO pins to read PPM and send PWM outputs to upto 12 pins. Check out erle robot Project which is an Open Hardware/Open Source quadcopter project using BBB running on ROS.


答:
I liked your choice for using BBB for your stepper motor driving project. BBB is the best open source hardware currently available to accelerate your stepper motor speed to upto 200Mhz precision which is like 15 Times faster than what we can get from Arduino or 12MHz Microcontroller. I used PyPruss python Library for my PRU based project where I coded my PRU program in ARM Assembly Language and generated the binary .bin file using PyPruss python Library. Here is an article which best suits for your project Accelerate Stepper Motor on BBB using Pypruss.

You can use the same thing using C++ PRU library, here are some example Assembly code which I wrote for my project:

C++ code to send PWM values to the shared memory (Memory shared by processor and PRU's).

Assembly code to read values from shared memory and generate realtime PWM values using PRU

Feel free to explore the repository.

Title: BeagleBone - PRU questions

Body:
I will be using at least one programmable real-time unit (PRU) to send pulses to a stepper motor driver but before I begin, I am trying to lay out the structure of my programs.

I am using this library PRU Linux API
for loading assembly code into the PRU instruction memory but there doesn't seem to be much documentation other then whats at that wiki and the source:
github-pru-packageh

My c program will be calculating the position of the sun using an algorithm and executing the assembly/writing a pulse count to the PRU(s) data memory so they can just switch on/off a gpio at my desired frequency and for the number of pulses required to turn a stepper the appropriate number of steps. I am not even sure if this is an acceptable method but I am pretty new at this and it seems like a simple way to accomplish my task

My Questions regarding the library functions are:

Is there a significant performance difference between using prussdrv_map_prumem or prussdrv_pru_write_memory to give the PRU(s) access to the pulse count?

Would it be better halt the PRU assembly program after has completed the tasks for each pulse count then re-execute it with new values, or keep the PRU program running and poll for a new pulse count to be written in?

I plan to send a pulse count every 10 seconds or so.

Any suggestions on revisiting the whole structure and logic are welcome as well.


问:
Can we use this line sensor as a proximity sensor?
I have an RSL Line Sensor which is designed to distinguish black and white lines. It detects white surface and gives me digital 1 as output, with 0 in case of black, but the surface needs to be close to it.

As it uses infra-red-sensors, I wanted to use this sensor as a proximity sensor, to tell me if there is a white surface near it. Is it possible to do this?

I think the only problem here is that we need to increase it's range of giving 1. Currently, it gives 1 only when white surface is too close to the sensors. I want 1 even if the white surface is there at a bit more distance.

Also there is an adjustable screw there to adjust something, under which POT is written. I am working with an Arduino.

答:
This would not be a good idea. As you can see in the specs of your sensor, it has a maximum range of 50mm. It's going to give you lots of faulty outputs if you try to use it for larger distances. My suggestion is to use a line sensor as a line sensor.

Three types of sensors are widely used as proximity sensors:

IR sensors: Omits IR pulses and receives the reflection. The distance is calculated based on triangulation.

Pros:

Very Affordable.

Cons:

Not a good option for outdoors, because of high IR level in sunlight.

It has a noticeable lower range. For example, GP2Y0D21YK does not see obstacles closer than 10 cm.

Sonar(ultrasonic) sensors: Sends sound pulses and receives the echo. The distance is calculated based on time of travel.

Pros:

Affordable.

Can be used in outdoor environment.

More accurate than IR sensor with less noise.

Cons:

Does not work very well with sound absorbing objects.

Might see ghost objects, because of receiving response after multiple reflections of the sound wave.

LiDAR (laser range finder): Sends near IR, visible, or UV light pulses and measures the time of travel. The sensors come in one, two, and three dimensional.

Pros:

Very Accurate.

Works with a wide range of surfaces, outdoor or indoor.

Cons:

Not as affordable as the other two.


答:
For gathering depth information and in general for mapping, there is no perfect solution yet. You need to have a good idea about application before choosing a sensor. Try to narrow down your options. Are you going to use it indoor or outdoor? How fast you want the depth measurements (you can use a single camera, but it will take around 10 seconds to generate a good point cloud)? What is the range? How accurate you want it to be (Lidar)? What resolution you need (e.g. 320x240)? What is your processing capabilities? Weight limits? Are you going to have vibration (no Lidar then)?

Configuration of stereo-vision sensor directly affects the range and accuracy. You can increase the distance between two cameras to get more reliable measurements for larger distances. It's not too difficult to make your own stereo-vision sensor. The only tricky part is two levels of calibration. One is for flatting the recorded images of each camera and the other is calculating geometric translation relation between two cameras. You can also add more cameras to your system. For example, there is a PointGrey sensor that uses 3 cameras using the same principle used in stereo-vision.

Time-of-flight cameras are generally smaller and lighter. Faster to process and give more information (e.g. color). They are not good options for outdoor or applications requiring long ranges. Emitted light might reflect more than once (does not happen too often like in sonar sensors) and give inaccurate measurements.

Speaking of sonars, you might want to look at side-scan sonars.


问:
In the PID equation K[((s+a)^2)/s] what values correspond to the PID coefficients Kp, Ki, Kd?
I'm trying to understand how to obtain the Kp, Ki, Kd values after finding a combination of K and a that works for me. Do I just expand the equation and take the coefficients? </p>

答:
The general PID form in Laplace domain is:
$$
G(s)=\frac{K_ds^2+K_ps+K_i}{s}
$$
The equation you've mentioned has a $Ks^3$ in the numerator. Hence, it cannot be transformed into PID form unless $K=0$, but that does not do you any good I guess.</p>


问:
Check if task exists in Not eXactly C
Is there a way to check if a task, function or variable exists in Not eXactly C?

I know that in PHP you can use isset() to check if a variable exists and function_exists() to do the same for a function, but is there a way to do that in NXC?

I am specifically interested in checking whether a task exists or it is alive.

答: No, it seems that there's no such possibility, because there's no method in the manual.</p>

Title: Check if task exists in Not eXactly C


问:
Does an electronic compass work underwater
I'm building a submersible ROV, so I need a way to navigate. So using a compass would help but this brings up the question, does an electronic compass work underwater?

My thoughts are the water might act as a faraday cage, and therefore interfere with the magnetic field. Therefore it might not even work. Maybe a gyroscope might be a better solution.

答: A compass will work just fine under water, I am an avid scuba diver and a compass is a standard piece of kit for navigating.</p>


问:
Drones and camera streaming
How do you stream video feed from a camera on a drone? I would think that at high altitudes Wi-Fi won't work.

So what would you usually do, and how?

答:
Common solutions are from those who fly FPV (First Person View), they use simple analog cameras connected to 900 MHz, 2.4 GHz, and 5 GHz the lower the frequency generally the longer the transmission distance. With a 1 W 900 MHz transmitter with a cloverleaf antenna (a common antenna type), and a 18 dB gain patch antenna directed at your craft you can easily get 5+ miles line of site.

If your asking about large commercial UAV systems, e.g. the predator, they use satellites to transmit and receive telemetry and video.

Title: Drones and camera streaming


问:
Why does microstepping give less torque?
I am experimenting with using a stepper motor for a robotics project. I'd like to use microstepping to give a better resolution and smoother movement, but I have noticed that the finer the microsteps, the lower the torque from the motor. Why is this?

For reference I'm using the Allegro Micro A4988 motor driver, and a bipolar stepper motor.

答: The reason for this is that when attempting to hold a position between positions, you are not only fighting the torque from your load but that generated by the magnets in your stepper more info here</p>


问:
Programming the Odometry of Rover 5
I have started in the programming stage of my project , and my first step is to made and test the odometry of my Rover 5 robot on Arduino Uno by using encoders to determine position and orientation .

I wrote this code and I don’t know if that code right or there are some mistakes,  because I am novice to Arduino and Robotic field so I need for some suggestions and corrections if  there were .

thanks a lot

Arduino codes posted below.

```<code>#define encoder1A  0       //signal A of left encoder  (white wire)
#define encoder1B  1      //signal B of left encoder  (yellow wire)
#define encoder2A  2      //signal A of right encoder  (white wire)
#define encoder2B  3      //signal B of right encoder  (yellow wire)

volatile int encoderLeftPosition = 0;      // counts of left encoder
volatile int encoderRightPosition = 0;     // counts of right encoder

float  DIAMETER  = 61  ;         // wheel diameter (in mm)
float distanceLeftWheel, distanceRightWheel, Dc, Orientation_change;
float ENCODER_RESOLUTION = 333.3;      //encoder resolution (in pulses per revolution)  where in Rover 5,  1000 state changes per 3 wheel rotations

int x = 0;           // x initial coordinate of mobile robot
int y = 0;           // y initial coordinate of mobile robot
float Orientation  = 0;       // The initial orientation of mobile robot
float WHEELBASE=183  ;       //  the wheelbase of the mobile robot in mm
float CIRCUMSTANCE =PI * DIAMETER  ;

void setup()
{
  pinMode(encoder1A, INPUT);
  digitalWrite(encoder1A, HIGH);       // turn on pullup resistor
  pinMode(encoder1B, INPUT);
  digitalWrite(encoder1B, HIGH);       // turn on pullup resistor
  pinMode(encoder2A, INPUT);
  digitalWrite(encoder2A, HIGH);       // turn on pullup resistor
  pinMode(encoder2B, INPUT);
  digitalWrite(encoder2B, HIGH);       // turn on pullup resistor
  attachInterrupt(0, doEncoder, CHANGE);       // encoder pin on interrupt 0 - pin 3
  Serial.begin (9600);
}

void loop()
{
  distanceLeftWheel = CIRCUMSTANCE * (encoderLeftPosition / ENCODER_RESOLUTION);       //  travel distance for the left and right wheel respectively
  distanceRightWheel = CIRCUMSTANCE * (encoderRightPosition / ENCODER_RESOLUTION);     // which equal to pi * diameter of wheel * (encoder counts / encoder resolution )
  Dc=(distanceLeftWheel + distanceRightWheel) /2 ;            // incremental linear displacement of the robot's centerpoint C
  Orientation_change =(distanceRightWheel - distanceLeftWheel)/WHEELBASE;    // the robot's incremental change of orientation , where b is the wheelbase of the mobile robot ,
  Orientation = Orientation + Orientation_change ;          //  The robot's new relative orientation
  x = x + Dc * cos(Orientation);                            // the relative position of the centerpoint for mobile robot
  y = y + Dc * sin(Orientation);
}

void doEncoder(){
  //  ---------- For Encoder 1 (Left)  -----------
  if (digitalRead(encoder1A) == HIGH) {   // found a low-to-high on channel A
    if (digitalRead(encoder1B) == LOW) {  // check channel B to see which way
                                             // encoder is turning
      encoderLeftPosition = encoderLeftPosition - 1;         // CCW
    }
    else {
      encoderLeftPosition = encoderLeftPosition + 1;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  {
    if (digitalRead(encoder1B) == LOW) {   // check channel B to see which way
                                              // encoder is turning
     encoderLeftPosition = encoderLeftPosition + 1;          // CW
    }
    else {
      encoderLeftPosition = encoderLeftPosition - 1;          // CCW
    }
  }
  //  ------------ For Encoder 2 (Right)-------------
  if (digitalRead(encoder2A) == HIGH) {   // found a low-to-high on channel A
    if (digitalRead(encoder2B) == LOW) {  // check channel B to see which way  encoder is turning
      encoderRightPosition = encoderRightPosition - 1;         // CCW
    }
    else {
      encoderRightPosition = encoderRightPosition + 1;         // CW
    }
  }
  else                                        // found a high-to-low on channel A
  {
    if (digitalRead(encoder2B) == LOW) {   // check channel B to see which way  encoder is turning
     encoderRightPosition = encoderRightPosition + 1;          // CW
    }
    else {
     encoderRightPosition = encoderRightPosition - 1;          // CCW
    }
  }
}
</code>```

答:
Your code won't work as you expect. The problem is that you use only one external interrupt, when you need two - one for each encode. With only one interrupt you will miss pulses from the second motor (for example when you brake one motor and still rotate the second to make a turn).

If you would like even better resolution, you can use two additinal interrupts, so you will have one for each encoder phase. If you are running short on external interrupt lines of uC, check out XOR logic ICs, or even dedicated quadrature decoders.

BTW. Lenght of the edge of a circle is circumference, not circumstance :)

Title: Programming the Odometry of Rover 5


答:
Actually, Rasbian can be turned into a real time OS. Before switching to ChibiOS and the like, you could give a try to Linux RT patch. There is a simple tutorial how to set up a RT kernel on RPi.

If the issue you're observing is caused by the scheduler jitter, the problem might go away.

I've managed to run a copter on 100Hz without a significant jitter.

Title: Raspberry Pi quadcopter thrashes at high speeds

Body:
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.


答: I know you said "not considering laser based detectors," but I am assuming you are discounting them due to price? There is a LIDAR unit available from sparkfun for less than $100 which claims to have a range to 40m. That's about half the price of a Kinect sensor. AFAIK, this unit only measures range to a single point, I'm not sure how easy it would be to implement it as a rastering scanner.</p>


问:
Why I'm getting very long terms in the inertia matrix (or dynamics model) of the robot using matlab script?
I'm working on the dynamics model of a RRRR articulated robot, I'm following Euler-Lagrange approach and developing my code in m-file in matlab; I'm looking for dynamic model of this form: $$
D(q) \ddot{q} + C(q,\dot{q})\dot{q}+ g(q) = \tau
$$
where $D$ and $C$ are $4 \times4$ matrices and $g$ and $\tau$ (torque) are $4\times1$ vectors; by formulating the kinetic and potential energies;;

The problem is that, I'm getting very long equations, and the term in $D$ matrix are very huge and nonlinear, involving sin and cos; I'm talking about a several pages per equation;
After I published the code - 7 pages - and the output I got around 45 pages in total;
I searched around there was some guy he faced the same problem before, but there was no helpful proposal.

Any Suggestions ??

答:
It really doesn't sound very unexpected for your equations to become this long. Something that might help a little, is using assumptions for defining symbolic variables.</p>

```<code>x = sym('x','real');
y = sym('y','positive');
z = sym('z','integer');
t = sym('t','rational');
</code>```

Body:
It really doesn't sound very unexpected for your equations to become this long. Something that might help a little, is using assumptions for defining symbolic variables.

x = sym('x','real');
y = sym('y','positive');
z = sym('z','integer');
t = sym('t','rational');


答:
As NXC is a compiled language, I would assume calling a non-existent function or accessing a non-existent variable would throw a syntax error during compilation as it does in other compiled languages I have used.

Conversely, in interpreted languages like PHP and Python you can often call or access things that don't exist without creating an issue until that actual call or access happens - hence the need for method's like isset() and function_exists() in PHP.

Edit: Looking at the Tasks docs it seems that a "task" is similar to a function in that it is defined in the source code, versus being created dynamically at run-time. I expect if you write something like: Precedes(non_existent_task_name); ("Precedes" being a function to start tasks), and try to compile you would trigger the same sort of syntax error as you would if you did something like: call_of_non_existent_function();

Title: Check if task exists in Not eXactly C

Body:
Is there a way to check if a task, function or variable exists in Not eXactly C?

I know that in PHP you can use isset() to check if a variable exists and function_exists() to do the same for a function, but is there a way to do that in NXC?

I am specifically interested in checking whether a task exists or it is alive.


问:
Stallable motor - 100% duty cycle higher torque motor that can be stalled without burning up
I have a project where I need a motor that can turn some number of rotations which will spool up a cable attached to a spring-closed device to open it up. When power is disconnected, the spring closure will cause the spool to unwind and the device to close.

In the closed position, no power is available. (i.e. The closure mechanism needs to be 100% passive.)

In order to keep this open for some time, I will need a motor that is capable of being stalled for long periods without having the windings burn up. I know some motors can do this, such as the motors they use on spring closed HVAC dampers, but I don't know how to find them or if there's a particular name I should be using to find them. I know I could probably do this with a stepper motor, but that seems overkill for the application.

The only requirements are higher torque to open this mechanism, no gearing that prevents the motor from spinning when power is disconnected, and the ability to be stalled.

答:
One simple alternative is to attach the spring in such a way that it doesn't pull the device closed when it is all the way open (see diagram below). In this fashion there is no torque acting on the device, so the motor will not need to be running at this point.

Note that this will require a small torque back in the original direction in order to close.


答: One possible option would be a linear solenoid. I would get a push type (Push solenoid's will push a rod out when energized and the builtin spring will auto retract it when the power is removed.)</p>

Title: Stallable motor - 100% duty cycle higher torque motor that can be stalled without burning up

Body:
I have a project where I need a motor that can turn some number of rotations which will spool up a cable attached to a spring-closed device to open it up. When power is disconnected, the spring closure will cause the spool to unwind and the device to close.

In the closed position, no power is available. (i.e. The closure mechanism needs to be 100% passive.)

In order to keep this open for some time, I will need a motor that is capable of being stalled for long periods without having the windings burn up. I know some motors can do this, such as the motors they use on spring closed HVAC dampers, but I don't know how to find them or if there's a particular name I should be using to find them. I know I could probably do this with a stepper motor, but that seems overkill for the application.

The only requirements are higher torque to open this mechanism, no gearing that prevents the motor from spinning when power is disconnected, and the ability to be stalled.


答: It actually works very good to a level that you can use fluxgate sensors for magnetic anomaly mapping. As far as I know, it has been done for Slocam (underwater glider) before. The map can be used for relative navigation of underwater systems. In your case, you can combine a magnetometer and an IMU for underwater navigation.</p>

Title: Does an electronic compass work underwater

Body:
I'm building a submersible ROV, so I need a way to navigate. So using a compass would help but this brings up the question, does an electronic compass work underwater?

My thoughts are the water might act as a faraday cage, and therefore interfere with the magnetic field. Therefore it might not even work. Maybe a gyroscope might be a better solution.


答:
I hope this won't sound too harsh but from my experience building a quadcopter and asking questions here this is exactly the kind of question which won't get you an answer that you'll find helpful. From what you describe there are far too many potential points of failures in your system for the community to contribute effectively. The people who have already commented with PID tuning tips or questions about the speed of your loop are making the huge assumption that everything else you have not talked about works.

I would suggest you test the different software modules independently and try to narrow down the problem. I appreciate you said you tested your sensors independently and they worked fine. Is that all you tested? How well does your attitude estimation algorithm work? What's its precision and its response time like? Does it still work when the motors spin (is it vibrations tolerant)? Can you control your motors directly (not through the PID)? What's their response time? Any glitches? What's the structure of your PID controllers: angle stabilisation, rate stabilisation, cascaded rate then angle stabilisation? Chances are you'll find the solution to your problem while answering one of these questions. And even if you don't you would then be able to ask a much more precise and answerable question.

Best of luck with your project in any case: quadcopters are hard!


问:
What are some low cost alternatives for lidar?
It need not be as effective as lidar or it may have some disadvantages when compared with lidar.   What are the probable alternatives?

Edit:
I'm intending to use it outdoors for navigation of autonomous vehicle. Is there any low cost LIDAR or is there any alternative sensor for obstacle detection?

答:
This representative sample of what's out there may give you some idea of what's out there at various price points:

Unfortunately, you're talking several thousand dollars for an outdoor unit with 10's to 100's of meter range (as of March 2015). The chart is from a blog article I wrote on the topic. Google used a $70-80K unit on their original vehicles. The 2nd to last item on the chart above, while not as capable as that unit, is advertised as a unit for vehicle automation.

Sophisticated stereo cameras with image processing may be an alternative, and may be cheaper, but you're still talking significant $, not just a webcam and OpenCV.


问:
Open Source software for Quadcopters
my question is general so please bear with me. I'm now interested in buying a quadcopter and develop some functions that it does for example an android app to control it, or objects detection.
So my question is what are the available quadcopters which has a software that allows me to do such things not just a flying toy?

P.S: I'm asked to buy a kit within 600$ and not build it by myself

答:
On the controller side, the following is a great starting point: http://ros.org/.

For drone itself, checkout http://diydrones.com/group/rosusergroup as they will know how to best integrate the controller with an off-the-shelf drone.

Cheers,

Title: Open Source software for Quadcopters