问:
How can I get the values of a IMU from the serial message received in Simulink via UART?
I try to read IMU sensor data from an Arduino mega 2560 UART with serial receive block of Arduino support package for simulink. The IMU can send binary packets and also nmea packets and I can configure it to any output. When the serial recieve block output is directly used, it displays just the numbers between 0-255. l need help about how to parse the coming data which contains the euler angles that I want to use.

Here is binary structure ;

"s","n","p",packet type(PT),Address,Data Bytes (D0...DN-1),Checksum 1,Checksum 0

The PT byte specifies whether the packet is a read or a write operation, whether it is a batch operation, and the length of the batch operation (when applicable). The PT byte is also used by the UM7 to respond to commands. The specific meaning of each bit in the PT byte is given below.

Packet Type (PT) byte;

7 Has Data,
6 Is Batch,
5 BL3,
4 BL2,
3 BL1,
2 BL0,
1 Hidden,
0 CF

Packet Type (PT) Bit Descriptions;

7...Has Data: If the packet contains data, this bit is set (1). If not, this bit is cleared (0).

6...Is Batch: If the packet is a batch operation, this bit is set (1). If not, this bit is cleared (0)

5:2..Batch Length (BL): Four bits specifying the length of the batch operation. Unused if bit 7 is cleared. The maximum batch length is therefore 2^4 = 16

1...Hidden: If set, then the packet address specified in the “Address” field is a “hidden” address. Hidden registers are used to store factory calibration and filter tuning coefficients that do not typically need to be viewed or modified by the user. This bit should always be set to 0 to avoid altering factory configuration.

0...Command Failed (CF): Used by the autopilot to report when a command has failed. Must be set to zero for all packets written to the UM7.

The address byte specifies which register will be involved in the operation. During a read operation (Has Data = 0), the address specifies which register to read. During a write operation (Has Data = 1), the address specifies where to place the data contained in the data section of the packet. For a batch read/write operation, the address byte specifies the starting address of the operation.
The "Data Bytes" section of the packet contains data to be written to one or more registers. There is no byte in the packet that explicitly states how many bytes are in this section because it is possible to determine the number of data bytes that should be in the packet by evaluating the PT byte.
If the Has Data bit in the PT byte is cleared (Has Data = 0), then there are no data bytes in the packet and the Checksum immediately follows the address. If, on the other hand, the Has Data bit is set (Has Data = 1) then the number of bytes in the data section depends on the value of the Is Batch and Batch Length portions of the PT byte.
For a batch operation (Is Batch = 1), the length of the packet data section is equal to 4*(Batch Length). Note that the batch length refers to the number of registers in the batch, NOT the number of bytes. Registers are 4 bytes long.
For a non-batch operation (Is Batch = 0), the length of the data section is equal to 4 bytes (one register). The data section lengths and total packet lengths for different PT configurations are shown below.
The two checksum bytes consist of the unsigned 16-bit sum of all preceding bytes in the packet, including the packet header.

Read Operations;

To initiate a serial read of one or more registers aboard the sensor, a packet should be sent to the UM7 with the "Has Data" bit cleared. This tells the device that this will be a read operation from the address specified in the packet's "Address" byte. If the "Is Batch" bit is set, then the packet will trigger a batch read in which the "Address" byte specifies the address of the first register to be read.
In response to a read packet, the UM7 will send a packet in which the "Has Data" bit is set, and the "Is Batch" and "Batch Length" bits are equivalent to those of the packet that triggered the read operation. The register data will be contained in the "Data Bytes" section of the packet.

here is an Example Binary Communication Code;

```<code>{
uint8_t Address;
uint8_t PT;
uint16_t Checksum;
uint8_t data_length;
uint8_t data[30];
}
UM7_packet;
// parse_serial_data.This function parses the data in ‘rx_data’ with length ‘rx_length’ and attempts to find a packet in the data. If a packet is found, the structure ‘packet’ is filled with the packet data.If there is not enough data for a full packet in the provided array, parse_serial_data returns 1. If there is enough data, but no packet header was found, parse_serial_data returns 2.If a packet header was found, but there was insufficient data to parse the whole packet,then parse_serial_data returns 3. This could happen if not all of the serial data has been received when parse_serial_data is called.If a packet was received, but the checksum was bad, parse_serial_data returns 4. If a good packet was received, parse_serial_data fills the UM7_packet structure and returns 0.

uint8_t parse_serial_data( uint8_t* rx_data, uint8_t rx_length, UM7_packet* packet )
{
uint8_t index;
// Make sure that the data buffer provided is long enough to contain a full packet The minimum packet length is 7 bytes
if( rx_length < 7 )
  {
  return 1;
  }
// Try to find the ‘snp’ start sequence for the packet
for( index = 0; index < (rx_length – 2); index++ )
  {
  // Check for ‘snp’. If found, immediately exit the loop
  if( rx_data[index] == ‘s’ && rx_data[index+1] == ‘n’ && rx_data[index+2] == ‘p’ )
    {
    break;
    }
  }
uint8_t packet_index = index;
// Check to see if the variable ‘packet_index’ is equal to (rx_length - 2). If it is, then the above loop executed to completion and never found a packet header.
if( packet_index == (rx_length – 2) )
  {
  return 2;
  }
// If we get here, a packet header was found. Now check to see if we have enough room left in the buffer to contain a full packet. Note that at this point, the variable ‘packet_index’contains the location of the ‘s’ character in the buffer (the first byte in the header)
if( (rx_length – packet_index) < 7 )
  {
  return 3;
  }
// We’ve found a packet header, and there is enough space left in the buffer for at least the smallest allowable packet length (7 bytes). Pull out the packet type byte to determine the actual length of this packet
uint8_t PT = rx_data[packet_index + 3];
// Do some bit-level manipulation to determine if the packet contains data and if it is a batch.We have to do this because the individual bits in the PT byte specify the contents of the packet.
uint8_t packet_has_data = (PT >> 7) & 0x01; // Check bit 7 (HAS_DATA)
uint8_t packet_is_batch = (PT >> 6) & 0x01; // Check bit 6 (IS_BATCH)
uint8_t batch_length = (PT >> 2) & 0x0F; // Extract the batch length (bits 2 through 5)
// Now finally figure out the actual packet length
uint8_t data_length = 0;
if( packet_has_data )
  {
  if( packet_is_batch )
    {
    // Packet has data and is a batch. This means it contains ‘batch_length' registers, each // of which has a length of 4 bytes
    data_length = 4*batch_length;
    }
  else // Packet has data but is not a batch. This means it contains one register (4 bytes)
    {
    data_length = 4;
    }
  }
else // Packet has no data
  {
  data_length = 0;
  }
// At this point, we know exactly how long the packet is. Now we can check to make sure we have enough data for the full packet.
if( (rx_length – packet_index) < (data_length + 5) )
  {
  return 3;
  }
// If we get here, we know that we have a full packet in the buffer. All that remains is to pullout the data and make sure the checksum is good. Start by extracting all the data
packet->Address = rx_data[packet_index + 4];
packet->PT = PT;
// Get the data bytes and compute the checksum all in one step
packet->data_length = data_length;
uint16_t computed_checksum = ‘s’ + ‘n’ + ‘p’ + packet_data->PT + packet_data->Address;
for( index = 0; index < data_length; index++ )
  {
  // Copy the data into the packet structure’s data array
  packet->data[index] = rx_data[packet_index + 5 + index];
  // Add the new byte to the checksum
  computed_checksum += packet->data[index];
  }
// Now see if our computed checksum matches the received checksum
// First extract the checksum from the packet
uint16_t received_checksum = (rx_data[packet_index + 5 + data_length] << 8);
received_checksum |= rx_data[packet_index + 6 + data_length];
// Now check to see if they don’t match
if( received_checksum != computed_checksum )
  {
  return 4;
  }
// At this point, we’ve received a full packet with a good checksum. It is already fully parsed and copied to the ‘packet’ structure, so return 0 to indicate that a packet was processed.
return 0;
}
</code>```

答: http://www.chrobotics.com/downloads/ExampleMatlabCode.zip read um7 directly from serial</p>


问:
Can i charge a lipo nano tech battery over imax b3 charger
Can i charge a lipo nano tech battery over imax b3 charger. 2650mah 35/70c 3s is the battery</p>

答:
Disclaimer: I have never actually used the B3 so trust this only 90% ;)

The iMax B3 is specifically marketed towards 2/3 cell Lipo Batteries and reviews suggest it works ok, so the B3 should be fine to use. As I mentioned in the comments, it can only supply 0.7A for 3 cells, so a 2.65Ah battery is going to take a very long time to charge.  Can the B3 damage the battery?  As long as you dont get faulty hardware (which is true for all chargers), and the fact you certainty won't have overcharging problems, it'll be completely safe.

I suggest investing in the iMax B6 as you can charge at a much faster rate and those will last you a decent amount of time (I've had mine for 2 years no problem)

ContentLicense: CC BY-SA 3.0


问:
Multiple EKFs or one big
Let's say I would like to use an EKF to track the position of a moving robot. The EKF would not only estimate the position itself but also variables affecting the position estimate, for example IMU biases, wheel radius, wheel slip and so on.

My question is, is it better to use one big EKF (state vector containing all estimated variables) or multiple smaller EKFs (each one responsible for tracking a subset of all variables to be estimated)? Or is there no difference?

As for the example above, the EKF could be split into one for tracking position, one for estimating wheel radius and slip and one for estimating IMU biases. The position EKF would of course use the estimations output from the other concurrent EKFs and vice versa.

To me it seems it would be easier to tune and test multiple smaller EKFs rather than just one big. Are there any other advantages/disadvantages (execution time, ease of debugging etc.) assuming the resulting estimates are equal in the two approaches (or close enough at least)?

Thanks,
Michael

答:
Unless there is some cross-channel noise, like a motor power line causing noise on some sensor signal line, then it won't matter if you do one large filter or a series of small filters because the noise covariance matrix and other matrices should all be block diagonal matrices.

As mentioned in this post over at the DSP stack exchange:

In general though your Q matrix will be full, not a diagonal, because there is correlation between the state variables. For example, suppose you are tracking position and velocity in x-axis for a car. Process noise (which Q is modelling) might be things like wind, bumps in the road, and so on. If there is a change in velocity due to this noise, there will also be a change in position. They are correlated, and so the off-diagonal elements will be non-zero for those terms.

So, while there may be correlation between position and velocity for one motor, there should be no correlation between feedback noise on one motor and feedback noise on the other motor, just like there shouldn't be correlation between motor feedback noise and IMU output, etc.

Given this, the fact that noise should not be correlated across various sensed elements, you are free to break the filter down into sub-sections. There shouldn't be any change in computational speed unless you're working with software highly optimized for matrices (read: Matlab), but even then there shouldn't be a noticeable difference in speed unless you're working with a huge (thousands of elements) matrix.

I would break the filter down into subsections just for clarity - you're going to have a bigger issue if you fat-finger or misinterpret a section and you don't notice because you're working with some ridiculous 15x15 matrix or similar.

Title: Multiple EKFs or one big


问:
USB instead of RS232
RS232 is not popular as it used to be and is mainly replaced by USB [wikipedia]. Problems such as mentioned in this question doesn't help its reputation either.

In a new system design therefore, one could think of using USB instead of Serial Port for communication. However, it still seems like RS232 is the serial communication protocol/port of choice.

Why is that? I understand changing old machinery that work with RS232 is costly, but what prevents new system designers from using USB instead of RS232?

答:
Simplicity for hardware designers and easy usage for programmers

Reliability

Well known and supported in many systems

Specially in robotics when we forced to use USB instead of RS232 (Our new PCs did not have RS232 ports). We faced many problems:

Any USB socket disconnection need so much effort to recover the system. Sometimes it leads to a bad system-fault and we had to restart PCs.

Any shock or hazard make it unplugged.

Too much programming time

Title: USB instead of RS232


问:
recommendation for really high precision attitude measurement sensors
I am new in this field, I am looking for some high precision gyroscopes and accelerometers for attitude measurements.The precision requirement is around 0.2~0.5 deg/s dynamic.
    I have done some digging myself, not a single integrated MEMS sensor can do that without costing too much. So some heavy math is needed but that's fine.I need to make sure the prefect sensors are chosen, the budget is less than 100USD.
    can any one help, thanks in advanced.</p>

答:
A friend of mine is senior developer at Infineon technologies for automotive applications. He told me that the best Inertial-MEMS for the automotive applications (which is basically your price target) are provided by BOSCH.
After a quick search, I found the SMI130 which has pretty good signal-to-noise ratios. In addition this chip is available for 13$.</p>


问:
Why do we generally prefer DH parameters over other kinematic representations of robot arms?
I am specifically interested in DH parameters versus other representations in terms of kinematic calibration.  The best (clearest) source of information I could find on kinematic calibration is in the book "Robotics: Modelling, Planning and Control" by Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo, chapter 2.11.  Which requires a description of the arm in DH parameters, multiplying out the kinematics equation, partial differentiation w.r.t. each DH parameter, then a least-squares fit (with the left pseudo-inverse), then iterate.

Is there some fundamental reason why DH parameters are used instead of a different representation (like xyz + euler angles).  I understand that there are fewer parameters (4 versus 6 or more), but for a calibration procedure like this I will be taking much more data than unknowns anyway.  All the robotics textbooks i have read just present DH parameters and say "this is what you should use", but don't really go into why.  Presumably this argument can be found in the original paper by Denavit, but I can't track it down.

答:
I have been doing a lot of reading up on kinematic calibration and here is what I found:

From [1]:

A kinematic model should meet three basic requirements for
  kinematic-parameter identification:

1) Completeness: A complete model must have enough parameters to describe any possible deviation of the actual kinematic parameters from the nominal values.

2) Continuity: Small changes in the geometric structure of the robot must
  correspond to small changes in the kinematic parameters. In
  mathematics, the model is a continuous function of the kinematic
  parameters.

3) Minimality: The kinematic model must include only a
  minimal number of parameters. The error model for the kinematic
  calibration should not have redundant parameters.

While DH parameters are complete and minimal, they are not continuous.  In addition, there is a singularity when two consecutive joints have parallel axes.  From [2]:

Our assumption is that small variations in the position and orientation of two consecutive links can be modeled by small variations of the link parameters.  This assumption is violated if we use the Denavit and Hartenberg link geometry characterization when the two consecutive joints have parallel or near parallel axes.

This has led a number of researchers to propose alternative models.  Namely the Hayati model [2], Veitschegger and Wu’s model [3], Stone and
Sanderson’s S-model [4], and the "Complete and Parametrically Continuous" (CPC) model [5].

These models typically involve adding parameters.  Which creates redundancy that has to be dealt with.  Or they are specifically tailored to the geometry of their robot.  Which eliminates generality.

One alternative is the Product of Exponentials formulation [6].  The kinematic parameters of in POE model vary smoothly with changes in joint axes and can handle kinematic singularities naturally.  However, due to the use of joint twists, this method is not minimal.  This led Yang et al. [7] to propose a POE formulation with only 4 parameters per joint which is minimal, continuous, complete, and general.  They do this by choosing joint frames very specifically.  (Which actually vaguely resemble D-H frames).

[1]: Ruibo He; Yingjun Zhao; Shunian Yang; Shuzi Yang, "Kinematic-Parameter Identification for Serial-Robot Calibration Based on POE Formula," in Robotics, IEEE Transactions on , vol.26, no.3, pp.411-423, June 2010

[2]: Hayati, S.A., "Robot arm geometric link parameter estimation," in Decision and Control, 1983. The 22nd IEEE Conference on , vol., no., pp.1477-1483, - Dec. 1983

[3]: W. Veitschegger and C. Wu, “Robot accuracy analysis based on kinematics,” IEEE Trans. Robot. Autom., vol. RA-2, no. 3, pp. 171–179, Sep.
1986.

[4]: H. Stone and A. Sanderson, “A prototype arm signature identification
system,” in Proc. IEEE Conf. Robot. Autom., Apr. 1987, pp. 175–182.

[5]: H. Zhuang, Z. S. Roth, and F. Hamano, “A complete and parametrically
continuous kinematic model for robot manipulators,” IEEE Trans. Robot.
Autom., vol. 8, no. 4, pp. 451–463, Aug. 1992.

[6]: I. Chen, G. Yang, C. Tan, and S. Yeo, “Local POE model for robot kinematic calibration,” Mech. Mach. Theory, vol. 36, no. 11/12, pp. 1215–
1239, 2001.

[7]: Xiangdong Yang, Liao Wu, Jinquan Li, and Ken Chen. 2014. A minimal kinematic model for serial robot calibration using POE formula. Robot. Comput.-Integr. Manuf. 30, 3 (June 2014), 326-334.


问:
Wall following using hokuyo lidar and sharp IR sensors
I have a mobile robot and I would like it to follow the walls of a room.

I have:

A map of the room.

Wheel encoders for the odometry.

A Kalman filter for fusing data from wheel encoders and IMU.

A Hokuyo lidar for localization and obstacle avoidance

A Kinect to see obstacles which can not be seen by the Hokuyo.

Amcl for localization.

A couple of sharp sensors on the side for wall following.

I am not planning to use the global or local costmap because the localization of the robot is not perfect and the robot might think that it is closer (or further away) to the wall than it actually is and therefore, wall following might fail. So, I am planning to just use the data from Hokuyo lidar and sharp sensors to do wall following and maintain constant distance from the wall (say 10 cm).

Now, I would like to know what is the best technique for doing wall following in this manner? Also, how can one deal with the issue of open gaps in the wall (like open doors, etc..) while doing wall following using the above approach?

I know this is a very general question but any suggestions regarding it will be appreciated. Please let me know if you need more information from me.

Update:

I am just trying to do wall following in a given room (I have the vertices of the room in a global reference frame) For example, Lets say I have a map of a room (shown below). I want to make the robot follow the wall very closely (say 10 cm from the wall). Also, if there is an open space (on bottom left), the robot should not go in the adjacent room but should keep on doing wall following in the given room (For this, I have the boundary limits of the room which I can use to make sure the robot is within the given room).

The approach which I am thinking is to come up with an initial global path (set of points close to the wall) for wall following and then make sure robot goes from one point to the next making sure that it always maintains a certain distance from the wall. If there is no wall, then the robot can just follow the global path (assuming localization is good). I am not sure about its implementation complexity and whether there is a better algorithm/ approach to do something like this.

答:
For a first attempt I would suggest constructing an angle histogram. See page 188-191 of this (old but still useful) reference:
http://www-personal.umich.edu/~johannb/Papers/pos96rep.pdf

In other words, construct straight line segments representing parts of the wall from your scan points.  Then identify the nearest wall and estimate its direction from the histogram, or perhaps by directly taking scan points along that region of the wall and making a least squares fit or some other average.

Once you have an approximate straight line for the wall, it should be possible to produce a turn command for the vehicle to steer itself.


问:
Why the name "combinatorial"?
Why are 'cell decomposition' methods in motion planning given the name, "combinatorial" motion planning?</p>

答:
Let's take a look what combinatorics is all about:

Combinatorics is a branch of mathematics concerning the study of finite or countable discrete structures.

Cell decomposition (or any graph based motion planning for that matter) operates on some finite structure. That sounds plausible, as choosing from infinitely many options can be a hard thing to do.

Note that the countable discrete structure can contain elements that in some sense represent infinitely many choices. Say for example if each cell is a polygon and you want to find a path from one side to another one, there are infinitely many ways to do this, even if you use a straight line to connect the starting and end point. That's because each side of a polygon is a line and thus consist of infinitely many points that you could start from.

But the general structure is still finite, because you have a finite amount of cells.

Aspects of combinatorics include counting the structures of a given kind and size (enumerative combinatorics),

If they are countable, then they can be counted. It sounds trivial, but it is very important information. If you want to implement cell decomposition, you want to know what kinds of cells to expect and how many there are.

Are there 20 polygons with up to 6 sides each? Or 15000 cells given
as analytical equations? Your choices for memory, processor and
algorithms will likely be different for the two above.

You might have some concave cells. Maybe you want to decompose them
further into convex cells. That increases the amount of cells that
you are dealing with.

Some cells could be so small that your robot couldn't even enter
them. Or could only access them from some sides. It the motion cannot
happen on some cells, it doesn't make too much sense to include them
for motion planning.

deciding when certain criteria can be met, and constructing and analyzing objects meeting the criteria (as in combinatorial designs and matroid theory)

Such criteria could be

the length of the path

the time it takes to travel along the path

the necessary turns

the width of the smallest passage in the path, if you want to carry that big box for example

finding "largest", "smallest", or "optimal" objects (extremal combinatorics and combinatorial optimization)

That seems to be the obvious one. It's often desirable to find the shortest path. But the shortest path is not necessarily the quickest one. Parcel delivery services for example calculate their routes by avoiding left turns in big cities. Maybe there's a path that's energetically optimal.

If there's a traffic jam on the freeway, what alternative route should people choose? It's likely a good idea to spread the throughput of traffic across many alternative paths, because a single road could not handle the amount of cars from a traffic jam on the freeway.

Title: Why the name "combinatorial"?


答: I'd guess it's because some use is made of the vocabulary (and perhaps even the tools) of the branch of mathematics called "combinatorial topology".</p>

Title: Why the name "combinatorial"?

Body: Why are 'cell decomposition' methods in motion planning given the name, "combinatorial" motion planning?</p>


问:
Axis of rotation via IMU
Using an IMU (gyro, accelerometer and magnetometer), as found in most smartphones, can I detect the differences between tilting the device, say forward, along different (parallel) axis positions?

To clarify, if the axis of rotation is far from the sensor, the the motion contains a translational component.

Can the distance and position of this axis be extracted from the IMU data and if so how?

Is there some data fusion algorithm that can do all this?

答:
Yes, in theory, from the gyro information. The accelerometers will give you orientation information, but won't help you interpret where the axis has been translated to.

Keep in mind though, that data from the gyros are time based and you will only have accurate information for the moments that you actually sample the data. I believe it will be difficult to get accurate enough info without using a very high sampling rate, but theoretically it is possible.

Title: Axis of rotation via IMU


问:
Positioning Sensor
I would like to locate the position of a stationary autonomous robot in x-y-z axis relative to a fixed starting point.

Could someone suggest sensors that would be suitable for this application?

I am hoping to move the robot in 3D space and be able to locate it's position wirelessly. The rate of position update is not important as I would like to stop the robot from moving and relay the information wirelessly.

The range I am looking for is roughly 2 KM + (the more the better) with accuracy of  +/- 1 CM.

Is there any system that could do this? Thanks for your help.

答: Feel free to correct me, or add more if you feel the need and have experience implementing this, but I believe instead of blowing tons of money on an ultra-precise Diff. GPS you could purchase a precision altimeter and then use some form of directional transmitter/receiver(laser?) and get your x,y,z from a sensor fusion such as this. Just an outside of the box thought instead of jumping straight to a very expensive GPS.</p>

Title: Positioning Sensor


问:
Do "nano bots" (that can fit inside the human body) actually exist?
I was wondering, do we have real nano bots, like the ones in the movies?

I think we have bots which can move through the blood vessels, am I right?

答:
The branch of science that most closely resembles "nano bots" is Capsule Endoscopy.

The first generation of these were just "edible" video cameras with cameras inside, which passively recorded one's digestive tract.   The most recent round of development is focusing on making them smaller and more maneuverable.

They are still not small enough to pass through blood vessels (at least, not safely).


问:
Tuning PD for line follower
I am trying to make line following robot. I am using atmega328p mcu, pololu 10:1 motors, pololu qtr6-rc sensor, 2s li-po. Here is my code:

```<code>/*
* LineFollower.c
*
* Created: 30.04.2015 16:00:05
*  Author: Mikk
*/
#define F_CPU 20000000         //we're running on 20mHz clock

#define numberOfButtons 1

#define READPORT    PORTC
#define READDDR     DDRC
#define READPIN     PINC        // lines connected to PC0 - PC5

#define MAXTICKS    2500
#define QTRCNT      6

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <Mikk/Button.h>
#include <Mikk/QTRRCSensors.h>

int baseSpeed = 70;
int maxSpeed = 140;
const float Kp = 8.1;
const float Kd = 400;

uint8_t mode = 0;                //indicates in which mode program is

uint8_t RmotorSpeed = 0;         //
uint8_t LmotorSpeed = 0;         //motors

void button(void);

void setMotors(int ml, int mr)
{
    if(ml > maxSpeed)             //make sure that speed is not out of range for left motor
        ml = maxSpeed;
    if(ml < -maxSpeed)
        ml = -maxSpeed;

    if(mr > maxSpeed)             //make sure that speed is not out of range for right motor
        mr = maxSpeed;
    if(mr < -maxSpeed)
        mr = maxSpeed;

    if(ml > 0)                    //if left motor speed is positive then drive motor forwards
        LmotorSpeed = ml;
    if(ml == 0)                   //if left motor speed is 0 then stop motor
        LmotorSpeed = 0;

    if(mr > 0)                    //if right motor speed is positive then drive motor forwards
        RmotorSpeed = mr;
    if(mr == 0)                   //if right motor speed is 0 then stop motor
        RmotorSpeed = 0;
}

void emittersOn(void)            //function for turning emitters on
{
    PORTD |= (1 << PIND0);
}

void emittersOff(void)           //function for turning emitters off
{
    PORTD &= ~(1 << PIND0);
}

void LedOn(void)                 //function for turning led on
{
    PORTB |= (1 << PINB5);
}

void LedOff(void)               //function for turning led off
{
    PORTB &= ~(1 << PINB5);
}

void stop(void)                 //stop everything
{
    LedOff();
    setMotors(0, 0);
    emittersOff();
}

void calibration(void)          //calibration takes about 5 seconds
{
    //turn led on
    LedOn();
    //turn emitters on
    emittersOn();
    // reset minimums and maximums
    for (int i = 0; i < QTRCNT; i++)
    {
        QTRmax[i] = 0;
        QTRmin[i] = MAXTICKS;
    }
    //calibrate sensors
    for(int i=0; i<250; i++)
    {
        calibrateQTRs();
        _delay_ms(5);
    }
    //turn emitters off
    emittersOff();
    //turn led off
    LedOff();
}

void start(void)
{
    //turn led on
    LedOn();

    //create all necessary variables
    int power_difference = 0;
    float error = 0;
    float lastError = 0;
    float derivative = 0;
    int position = 0;

    //turn emitters on
    emittersOn();
    _delay_ms(500);               //wait so you can pull your hand away
    while(mode == 2)
    {
        //check for mode change
        button();

        //read position
        position = readLine();
        //make calculations
        error = position - 2500;
        derivative = error - lastError;

        //remember last error
        lastError = error;

        //calculate power_difference of motors
        power_difference = error/(Kp/100) + derivative*(Kd/100);

        //make sure that power difference is in correct range
        if(power_difference > baseSpeed)
            power_difference = baseSpeed;
        if(power_difference < -baseSpeed)
            power_difference = -baseSpeed;

        //drive motors
        if(power_difference > 0)
            setMotors(baseSpeed+power_difference, baseSpeed-power_difference/2);
        else if(power_difference < 0)
            setMotors(baseSpeed+power_difference/2, baseSpeed-power_difference);
        else if(power_difference == 0)
            setMotors(maxSpeed, maxSpeed);
    }
}

void button(void)
{
    char buttonState = 0;
    //check for current button status
    buttonState = ButtonReleased(0, PINB, 1, 200);
    //check if button is pressed
    if(buttonState) //pin change from low to high
    {
        mode++;
        if(mode == 1) calibration();
    }
}

void pwmInit(void)
{
    //set fast-PWM mode, inverting mode for timer0
    TCCR0A |= (1 << COM0A1) | (1 << COM0A0) | (1 << WGM00) | (1 << WGM01) | (1 << COM0B1) | (1 << COM0B0);
    //set fast-PWM mode, inverting mode for timer2
    TCCR2A |= (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM21) | (1 << COM2B1) | (1 << COM2B0);
    //set timer0 overflow interrupt
    TIMSK0 |= (1 << TOIE0);
    //set timer2 overflow interrupt
    TIMSK2 |= (1 << TOIE2);

    //enable global interrupts
    sei();
    //set timer0 prescaling to 8
    TCCR0B |= (1 << CS01);
    //set timer2 prescaling to 8
    TCCR2B |= (1 << CS21);
}

int main(void)
{
    DDRB |= 0x2A;                 //0b00101010
    DDRD |= 0x69;                 //0b01101001
    DDRC |= 0x00;                 //0b00000000

    //clear port d
    PORTD |= 0x00;

    //enable pull-up resistor
    PORTB |= (1 << PINB1);

    initQTRs();
    pwmInit();

    //blink 2 times indicate that we are ready
    for(int i=0; i<4; i++)
    {
        PORTB ^= (1 << PINB5);
        _delay_ms(500);
    }

    while(1)
    {
        button();
        if(mode == 0) stop();
        if(mode == 2) start();
        if(mode >= 3) mode = 0;
    }
}

//update OCRnx values
ISR(TIMER0_OVF_vect)
{
    OCR0A = RmotorSpeed;
}
ISR(TIMER2_OVF_vect)
{
    OCR2A = LmotorSpeed;
}
</code>```

And here is my qtr library:

```<code>    #ifndef QTRRCSensors
#define QTRRCSensors

#define SLOW        1
#define FAST        0

static inline void initQTRs(void)
{
    TCCR1B = (1 << CS11);
}

uint16_t QTRtime[QTRCNT], QTRmax[QTRCNT], QTRmin[QTRCNT];

static inline void readQTRs(uint8_t forceSlow) {
    uint8_t lastPin, i, done = 0;

    for (i = 0; i < QTRCNT; i++)                    // clear out previous times
        QTRtime[i] = 0;

    READDDR |= 0b00111111;                          // set pins to output
    READPORT |= 0b00111111;                         // drive them high

    _delay_us(10);                                  // wait 10us to charge capacitors

    READDDR &= 0b11000000;                          // set pins to input
    READPORT &= 0b11000000;                         // turn off pull-up registers

    TCNT1 = 0;                                      // start 16bit timer at 0
    lastPin = READPIN;

    while ((TCNT1 < MAXTICKS) && ((done < QTRCNT) || forceSlow))     // if forceSlow, always take MAXTICKS time
    {
        if (lastPin != READPIN)                     // if any of the pins changed
        {
            lastPin = READPIN;
            for (i = 0; i < QTRCNT; i++)
            {
                if ((QTRtime[i] == 0) && (!(lastPin & (1<<i))))    // did pin go low for the first time
                {
                    QTRtime[i] = TCNT1;
                    done++;
                }
            }
        }
    }
    if (done < QTRCNT)                              // if we timed out, set any pins that didn't go low to max
        for (i = 0; i < QTRCNT; i++)
            if (QTRtime[i] == 0)
                QTRtime[i] = MAXTICKS;
}

void calibrateQTRs(void) {
    uint8_t i, j;

    for (j = 0; j < 10; j++) {                      // take 10 readings and find min and max values
        readQTRs(SLOW);
        for (i = 0; i < QTRCNT; i++) {
            if (QTRtime[i] > QTRmax[i])
                QTRmax[i] = QTRtime[i];
            if (QTRtime[i] < QTRmin[i])
                QTRmin[i] = QTRtime[i];
        }
    }
}

void readCalibrated(void) {
    uint8_t i;
    uint16_t range;

    readQTRs(FAST);

    for (i = 0; i < QTRCNT; i++) {                  // normalize readings 0-1000 relative to min & max
        if (QTRtime[i] < QTRmin[i])                 // check if reading is within calibrated reading
            QTRtime[i] = 0;
        else if (QTRtime[i] > QTRmax[i])
            QTRtime[i] = 1000;
        else {
            range = QTRmax[i] - QTRmin[i];
            if (!range)                             // avoid div by zero if min & max are equal (broken sensor)
                QTRtime[i] = 0;
            else
                QTRtime[i] = ((int32_t)(QTRtime[i]) - QTRmin[i]) * 1000 / range;
        }
    }
}

uint16_t readLine(void) {
    uint8_t i, onLine = 0;
    uint32_t avg;                                   // weighted total, long before division
    uint16_t sum;                                   // total values (used for division)
    static uint16_t lastValue = 0;                  // assume line is initially all the way left (arbitrary)

    readCalibrated();

    avg = 0;
    sum = 0;

    for (i = 0; i < QTRCNT; i++) {                  // if following white line, set QTRtime[i] = 1000 - QTRtime[i]
        if (QTRtime[i] > 50) {                      // only average in values that are above a noise threshold
            avg += (uint32_t)(QTRtime[i]) * (i * 1000);
            sum += QTRtime[i];
            if (QTRtime[i] > 200)                   // see if we're above the line
                onLine = 1;
        }
    }

    if (!onLine)
    {
        // If it last read to the left of center, return 0.
        if(lastValue < (QTRCNT-1)*1000/2)
            return 0;

        // If it last read to the right of center, return the max.
        else
            return (QTRCNT-1)*1000;

    }

    lastValue = avg/sum;                            // no chance of div by zero since onLine was true

    return lastValue;
}

#endif
</code>```

I am trying to find Kp constant but when it's 7 then my robot just turns off the line always on the same spot. When Kp is 8 then it follows staright line but wobbles a lot and can't take corners. I also tried to increase Kd 10 to 20 times when my Kp was 8 but it didn't change much. How can I get it working?

Here is my robot and the track I want to follow.

答:
Your PID calculation is all wrong. You have:

```<code> //calculate power_difference of motors
power_difference = error/(Kp/100) + derivative*(Kd/100);
</code>```

First,  you need correct error terms. You calculate:

```<code>//make calculations
error = position - 2500;
derivative = error - lastError;
</code>```

I'm assuming here that 2500 is the signal you are expecting if you are exactly centered on the line. Even if this is the case, you should be calculating error as:

```<code>error = reference - output;
</code>```

You have, assuming 2500 is the ideal line reading,

```<code>error = output - reference;
</code>```

which is incorrect. This is flipping the sign on your error term.  You then calculate derivative error as the difference between current and previous error, which is also incorrect. You need the sample time, loop time, sweep time, etc. - the time that elapses between when you calculate the error terms. You can do this with a timer for an exact measurement or you can use a pause or delay command at the end of the loop for an approximate measurement.

Once you have your sample time, your derivative error is calculated by:

```<code>derivativeError = (error - previousError)/sampleTime;
</code>```

Now your control signal is just proportional gain times error and derivative gain times derivative error. You are dividing by your gains and then dividing those gains each by 100.

Where you have:

```<code>power_difference = error/(Kp/100) + derivative*(Kd/100);
</code>```

You should have:

```<code>power_difference = error*Kp + derivative*Kd;
</code>```

Honestly I'm surprised you managed to get it to do anything at all. Putting it all together, I would suggest something like:

```<code>sampleTime = 50;    // 50 ms sample time
while(mode == 2)
{
    //check for mode change
    button();

    //read position
    position = readLine();
    //make calculations
    error = 2500 - position;
    derivative = (error - lastError)/sampleTime;

    //remember last error
    lastError = error;

    //calculate power_difference of motors
    power_difference = error*Kp + derivative*Kd;

    //make sure that power difference is in correct range

    //drive motor
    delay (sampleTime)
}
</code>```

Adjust sample time down if you find you absolutely need a faster response, but know that the lower you set it the more inaccurate you get because calculation time takes up a proportionally bigger time.

Finally, reset your gains and tune again. Check out Ziegler-Nichols method for some help tuning your controller once you've made the code changes.

Title: Tuning PD for line follower

Body:
Your PID calculation is all wrong. You have:

 //calculate power_difference of motors
power_difference = error/(Kp/100) + derivative*(Kd/100);

First,  you need correct error terms. You calculate:

//make calculations
error = position - 2500;
derivative = error - lastError;

I'm assuming here that 2500 is the signal you are expecting if you are exactly centered on the line. Even if this is the case, you should be calculating error as:

error = reference - output;

You have, assuming 2500 is the ideal line reading,

error = output - reference;

which is incorrect. This is flipping the sign on your error term.  You then calculate derivative error as the difference between current and previous error, which is also incorrect. You need the sample time, loop time, sweep time, etc. - the time that elapses between when you calculate the error terms. You can do this with a timer for an exact measurement or you can use a pause or delay command at the end of the loop for an approximate measurement.

Once you have your sample time, your derivative error is calculated by:

derivativeError = (error - previousError)/sampleTime;

Now your control signal is just proportional gain times error and derivative gain times derivative error. You are dividing by your gains and then dividing those gains each by 100.

Where you have:

power_difference = error/(Kp/100) + derivative*(Kd/100);

You should have:

power_difference = error*Kp + derivative*Kd;

Honestly I'm surprised you managed to get it to do anything at all. Putting it all together, I would suggest something like:

sampleTime = 50;    // 50 ms sample time
while(mode == 2)
{
    //check for mode change
    button();

    //read position
    position = readLine();
    //make calculations
    error = 2500 - position;
    derivative = (error - lastError)/sampleTime;

    //remember last error
    lastError = error;

    //calculate power_difference of motors
    power_difference = error*Kp + derivative*Kd;

    //make sure that power difference is in correct range

    //drive motor
    delay (sampleTime)
}

Adjust sample time down if you find you absolutely need a faster response, but know that the lower you set it the more inaccurate you get because calculation time takes up a proportionally bigger time.

Finally, reset your gains and tune again. Check out Ziegler-Nichols method for some help tuning your controller once you've made the code changes.


问:
How is a brushless gimbal motor different from a regular brushless motor?
How are the brushless motors in a gimbal assembly designed?

Obviously it doesn't need continual rotation, but it does need accurate control of precise position. I've noticed that the motors in my gimbal don't have the usual magnetic 'snap' positions that my other motors do.

What are the primary design differences in these kinds of motor, if any?

答:
I believe this blog has at least part of the answer to your question.

Brushless gimbal motor vs. Brushless Motor vs. Servo

A user here described the comparison as:

A brushless gimbal motor is a regular brushless motor but wound for very low speed (ie lots of turns of thin wire) and many do indeed have more poles.

Another post continues:

They are both low and high speed at the same time. Low speed, in that they never actually
  complete a full 360 rotation so you wouldn't normally think of measuring their speed in rpm,
  but high speed in that they need to respond instantly to sometimes very rapid changes
  in direction as the aircraft may be getting shook by vibrations and turbulence.
  That means they're meant to be light and fast, not powerful and slow.
  These motors are not meant to carry any kind of continuous load.

Generally, I think it can be said that a brushless gimbal motor is one specifically wound to be used in fine control applications. A brushless motor on the other hand might be considered more of a load bearing motor.

That being said, most brushless gimbal motors are used in applications that could otherwise be filled by servo motors: finely controlled applications.

I hope this helps.


问:
Ubuntu ARM lacking /sys/devices/cape-bone-iio
I'm trying to pull analog input from a beaglebone black using this tutorial. However when I go to /sys/devices there is no cape-bone-iio. I have spoken with several other programmers and one of them suggested that the cape-bone does not work with the newer versions of Linux. However downgrading could have negative impact on the rest of the project. Is there any other solution?

答:
I believe I encountered the exact same problem that you are encountering. The short/easy fix is downgrade your kernel. The newer kernel version (later than  3.8.x) for UbuntuARM does not use device tree overlays and stores all of these ADC values in god knows where.

You'll also notice that your tutorial is indeed using kernel 3.8.13. One that still involves device tree overlays and stores ADC values in the cape-bone-iio.

While it might not be exactly what you want, on my github I have a step by step approach to getting ADC values (as part of using ROS) on the BBB. This includes how to downgrade your kernel and enable Device Tree Overlays and then where the file locations for these values will be. I would recommend taking a look at it for some guidance towards your specific solution.

My Github

The steps for accomplishing this are directly on this page. I cannot post them directly due to a lack of reputation and the number of links the guide entails.

Another helpful hint, would be to specifically search for the name Robert C Nelson alongside Beaglebone Black. He has worked extensively with them and is involved in a multitude of blogs and tutorials that might help you find your solution.

Hope all of this helps.


问:
Total Hand calculations procedure & formulaes of Mega-Quadcopter
I am a student of BE taking Mega-Quadcopter as my final year project.Can u please help me with the total hand calculations of the mega-copter i.e its procedure and formulaes? . I wanted to know how to calculate the dimensions of frame,specifications of motor and propeller,the rating of ESC's and the power rating of the batteries and its total no.s.I do not want direct answers but its procedure and formulaes.I want to lift aload of around 20-30 kgs .Please feel free to help.</p>

答:
What is BE? Bachelor of Engineering? I feel like you shouldn't need our help if you're in your senior year of an engineering degree, but briefly:

It's all about the specifications. Pick a vertical acceleration you want to achieve, then calculate the total force (thrust) you need with $F=ma$.

Divide the total thrust by the number of motors you want to use for the per-motor thrust required.

Define a rotation rate you want to achieve then calculate the torque required to achieve that rate with the torque equation $\tau = I \alpha$.

Use the torque from this equation with the per-motor thrust from earlier to determine the motor radial distance with $\tau = r \times F$. This spacing will impact your moment if inertia, so you will have to re-calculate the required torque - this is called iterative design.

Define a top vertical speed, then calculate the required motor power with this speed and the per by $P = F v$. Then define a top rotational speed and calculate the power required for rotating by $P = \tau \cdot \omega$. The actual power you need will be the larger of the two power terms you calculate to ensure you can meet all of your performance specifications.

Using the calculated motor power will give you a ballpark to start shopping for motors. Contact sellers and give them your power and thrust requirements to see if they can provide something that meets your needs, but the motors are going to weigh something substantial, so you need to basically re-calculate everything from the moment of inertia down again to refine your design. Again, iterative design process.

You will be better off empirically testing motor/prop combinations than trying to calculate thrust from blade design, but again, your base calculations should give the class of motor you need by output power required. Also, use sales engineers! It's a LOT less work to give a sales engineer your spec and have them do the legwork than to do this testing yourself. Give them your spec, then ask to try their recommendation to verify before purchase.

You should find your motor power requirements in line with this guide posted by TobiasK. Using the moderate flight estimate of 250W/kg, you're looking at over 6kW just for your payload. When you add an airframe, motors, and batteries, you could be looking at 7.5 to 10kW - this is not a small project!


问:
Orthogonal projection of laserscanner data
I recently discovered this ROS-package: http://wiki.ros.org/laser_ortho_projector .
Which is basically exactly what I need. However I am not using ROS, so I need to do what is been done in this package myself.

Basically the information I have is the range measurement r and the angle theta for every measurement point of a 360 degree laserscan + I have the orientation in roll, pitch, yaw angles of the laserscanner. However yaw is not important for me and could be ignored.

I really can't get my head around how to project those points to the ground plane. I mean it is easy for the measurement point which align with the roll and pitch axes, but I don't know what to do with the points in between.

One solution I thought of is this:

Convert the measurement point (r, theta) in cartesian coordinates (x,y,z) - vector

Use rotations matrices: create rotation matrix for rotation around roll axis with roll angle, and adequately for the pitch axis. Multiplay bot matrices and then multiply it with (x,y,z) - vector.

Now the orthogonal projection of the of the measurement would be the (x,y,z) - vector with z=0.

Convert (x,y) - vector back to polar coordinates (r, theta).

However, especially step 2 is very complicated, because the rotation matrices change according to the sign of the roll and pitch angles, right?

I would like to note that the absolute value of role and pitch angles will always be < 90°, so there should not be an unambiguity with rotations..

Is there an easier (or maybe more elegant) way to solve my problem?

My guess is that this problem must have been solved basically for every robot application which uses a 2D-laserscanner that is not fixed to one axis.
But I can not find the solution anywhere.

So I would be very glad if anyone of you could point me in the right direction.

答:
Use the location and attitude measurements of your craft to develop a 4x4 transform matrix, then take the inverse of that matrix to put the points you measure back in terms of world coordinates. Then, if you want a projection just set the respective measurement to zero.

If you want a projection on the XY plane, set all Z coordinates to zero, set X to zero for a projection on the YZ plane, etc.

There are lots of ways to build the rotation matrix required for the transform matrix. You need to consult your IMU documentation to see how it is interpreting/outputting angle information to build the rotation matrix correctly.

As a final note, even if you don't need yaw information don't discard it. Depending on the IMU configuration yaw may not mean what you think it means and thus could have an impact on your transform matrix.


问:
What erector sets will function with normal servo motors?
I need a basic erector set that the parts will fit with servo motors and dc motors. Preferably below $100. I've looked at Minds-i basic set and it looks good except I don't know if it will function with my servos without hot glue or extensive modifications. </p>

<p>If it matters, I am making a bipedal robot so I don't require any wheels or anything pre-built. I just need a basic set that I can add on to to build a whole bunch of different robots. </p>

答:
First, you don't need to bump your post with edits. 6 new questions in a day seems to be pretty busy around here. You ask at 6pm on a Sunday - probably nobody was around.

Second, I don't think there is a "standard" servo mount, so no kit is going to give you what you're looking for. The mounting pattern varies by manufacturer and model so you need to pick a servo and look for a mount for that particular one.

Third, I'm not sure why you would go for an 85 dollar, 240 piece knock-off LEGO kit when a genuine 204 piece kit is 60 dollars. Genuine LEGO sets give you guaranteed compatibility with all of their other stuff, like motors, pneumatics, and the mindstorm equipment.

Fourth, I don't know why you are willing to invest so much in a kit to mount servos when the basic servo bracket costs between 2 and 5 dollars. For 85 dollars you could buy 15-40 servo brackets.

Fifth, if you leave LEGO kits behind and just go for a servo bracket, you can check out other kits that are good for building things. Ironically, your question states you are looking for "a basic erector set", but you link an 85 dollar, 240 piece generic LEGO kit when you could buy an actual 640 piece Erector set for 80 dollars.


问:
Air hockey with a robot as an opponent
I'm not sure if this is the right place to post this but here goes.

So, as the title states, I'm planning on building a desk that doubles as an air hockey table which has a robot on the other side.

The robot would be mounted on a rail which should be able to go left and right using a linear actuator. It should be able to "attack" the puck using two servos.

The real problem is how should I detect the puck's location?

My idea:

Since the table would have tiny holes in the corners of a every square(0.5inx0.5in), I could fit in a laser on the bottom part of the table, a laser for ever 1in so a 1inx1in square, the same location would be reflected on the "ceiling" of the table but instead of laser diodes, they would be replaced by an ldr.

So I'm planning on doing a matrix and reading the signals of the ldr's columns and rows then performing some logic to locate the center of the puck.

PROBLEMS:

While I don't see any performance flaws in my plan, I see tons of flaws when done imperfectly even to the tiniest bit.

I have to be exactly accurate regarding the laser diode's position,
it has to be on the center of the holes, right below the z-axis.
This should be easy if I'm just going to place 4 or 5. But I'm not.

According to my estimations, I'm going to have to use 300-700 laser
diodes, depending on if I'm planning on putting the lasers only on
the opponent's side or on the entire board. It would definitely be
costly. Imagine 300...

This isn't really a huge problem, more like a hassle. Wiring 300 of
these. Forget the pcbs, the project area is just to large.

I have thought of numerous way to lessen these, like using a color sensor to get the x-axis location and a laser situated on a negative x-axis pointing to the positive x-axis to locate the puck's y location, but I'm still comparing ideas.

Advantages:

I could get a 3d-like graphical representation with 3d-like controls (3d-like in reality but technically 2d since the lasers are only plotted in the x and y axis though facing the z-axis).

Since this project is going to be my room desk, situated in an automated room, I was thinking of making "desk modes" which should toggle between a game that takes advantage of the lasers and their controls, A control desk for my room, ordinary desk mode, and an air hockey mode.

My question: (More like a request)

Does anyone have another idea regarding how I should be able to locate the puck's x and y location accurately in real time?

EDIT: The table is roll-able and stored underneath a loft bed which has an under-area height of 5'4". Which means I can't go grande on the a vertical solution.

EDIT #2: Thanks to the helpful people here, I have come to the conclusion of using a camera.

The camera will be that of a smartphone's, I'll create an app that tracks an object by color and a has fixed size comparison to identify the distance of the robot from the puck. The phone will then process this and send signals via bluetooth.

The phone is anchored at the end of the robot's moving part so the camera is reminiscent of those games with a first-person view.

Incoming problems: I'm looking forward to some delay, given the delay in processing.

答:
A well lit, perfectly circular object on a contrasting background, like a puck on an air hockey table, is pretty much the ideal use for webcam object tracking.

A classic air hockey table has a light bar/score indicator across the middle of the table - just put the camera in there looking down.

Check out the OpenCV library and webcam tracking tutorials online.

Title: Air hockey with a robot as an opponent


问:
How to make door opening to the top
I need to make this construction (door is closed by default, door is opened to the top). This is the scheme:

Red rectangle on the picture is the aperture, blue rectangle is the door (weight is about 0.5 kg), which moves top when door need to be opened. Green stripe on the picture is the rail for the door.

Which electrical engine should I use?

Estimated time of door opening is about 10 seconds, I want to send signal to up the door, it should be drop down when power is lost or I should send a signal to drop it down.

答:
Using an engine is an exaggeration. Try using a high torque rotary motor, if it isn't enough introduce some gears to increase torque, though the speed might get slower.

But basing on your comment, your door only weighs 500 grams, so a medium torque stepper should do the job, or a medium torque servo motor, or a medium torque dc motor with a limit switch. (By medium, I mean enough to lift more than half a kilo but not necessarily a whole kilo).

Given the missing information, I think the  most ideal would be a stepper motor with a pulley-like system. Since you never told us the depth of the aperture and the height of the gap to be created then we can't easily call on a servo.

Regarding the transistor, you might wanna use a transistor to power a relay and not directly power the motor. A transistor might be faster and smaller than the relay but it isn't built to power mechanical systems unlike a relay. Though a relay should be easier to wire.

Title: How to make door opening to the top


问:
What PID values should I keep?
I have built quadcopter but the problem is of balancing. It doesn't goes up. I am using PID technique for balancing. But I am not finding the suitable values for PID tuning. I am using mpu6050 as a sensor. I get the accelerometer values of x and y axis and find the error from them. That is lets say if accel on x is not zero then it error cause it should be zero if balanced. I am using +-2g sensitivity scale of accelerometer. The motors I am using are DJI 920 kva. What values for kp, ki, and kd should I set? I cant set them while in flight cause it completely out of balance.

This is the design. Completely homemade. I have modified it a little after this photo. An accelerometer is at 2g so at balance z will be 32768/2.

```<code>short PID()
{
    short error,v;
    error = desired-current;
    //error/=390;
    integ += error;
    der = error - perror;
    x = error;
    x2 = integ;
    x3 = der;
    x* = kp;
    x2* = ki;
    x3* = kd;
    v = kpi;x/=100;
    v = kii;x2/=1000;
    v = kdi;x3/=1000;
    x = x+x2+x3;
    //x/=390;
    perror = error;
    return x;
}
</code>```

There are also few more questions, should I scale error or PID output? Because, the error is ranging from 0 to 16380 at 2g setting, so I am scaling it from 0 to 42. So should I divide error or PID by some value?

答:
I'm sorry but it is absolutely impossible to tell you what values you should put based on the information you gave. Actually to give a real answer we would need a complete model of the system (or an approximation at least...). There are some generic models for quadcopters but parameters depend on your own system.

If you want to really understand the command of a quadcopter I recommend to read some articles like this one (I found it very quickly, maybe some others are better, but it explains how to control a quadcopter with a PID). Or at least watch this video to understand what is a PID controler.

However, if you don't want to find a model for your system, the best way to find PID parameters is empirically. You just need to know what are the consequences of each coefficient, and try to adapt them according to your system's behaviour.
There is a very quick explanation for each coeff :

P : is mainly for response speed. Too low = correction will be to slow. Too high : correction will be very fast and oversized, and your system will probably oscillate.

I : is mainly used to cancel static error. Too low : your response will never reach the order. Too high : oscillations.

D : is... more complex to understand. It will impact your response variation. Look at the video to have a good explanation.

Good luck !

ContentLicense: CC BY-SA 4.0

Title: What PID values should I keep?


答:
In addition to Chuck's answer:

I recommend using opencv or any other Computer Vision library.

But you do not need to go the simple way. Do not place the camera directly above the table. You can place it everywhere you want, in any orientation you want. Because you do know your table, the cameras position and its attitude, you can use some easy trigonometrics to calculate the position of the puck. You know have a 3-Dimentional problem to solve, instead of the 2D Problem, which you have with a straight orientation, But it can be done easily by a computer. It is also possible to place multiple cameras, if they have a bad angle.

Another possibility (I will just mention this one, I do not recommend it) is using light barriers in the borders of your table, at all four. There are pretty long ones with multiple sensing points (resulotion is about 1 inch). At the end you still need some software which is estimating the current position and velocity of the puck from the information you get from light barrier. The disadvantage is that this light barriers cost a fortune. They easily cost more then a complete airhockey table. If you are interested in this solution look for light barriers used in automation industry for safety purposes in the handling area.

Title: Air hockey with a robot as an opponent

Body:
I'm not sure if this is the right place to post this but here goes.

So, as the title states, I'm planning on building a desk that doubles as an air hockey table which has a robot on the other side.

The robot would be mounted on a rail which should be able to go left and right using a linear actuator. It should be able to "attack" the puck using two servos.

The real problem is how should I detect the puck's location?

My idea:

Since the table would have tiny holes in the corners of a every square(0.5inx0.5in), I could fit in a laser on the bottom part of the table, a laser for ever 1in so a 1inx1in square, the same location would be reflected on the "ceiling" of the table but instead of laser diodes, they would be replaced by an ldr.

So I'm planning on doing a matrix and reading the signals of the ldr's columns and rows then performing some logic to locate the center of the puck.

PROBLEMS:

While I don't see any performance flaws in my plan, I see tons of flaws when done imperfectly even to the tiniest bit.

I have to be exactly accurate regarding the laser diode's position,
it has to be on the center of the holes, right below the z-axis.
This should be easy if I'm just going to place 4 or 5. But I'm not.

According to my estimations, I'm going to have to use 300-700 laser
diodes, depending on if I'm planning on putting the lasers only on
the opponent's side or on the entire board. It would definitely be
costly. Imagine 300...

This isn't really a huge problem, more like a hassle. Wiring 300 of
these. Forget the pcbs, the project area is just to large.

I have thought of numerous way to lessen these, like using a color sensor to get the x-axis location and a laser situated on a negative x-axis pointing to the positive x-axis to locate the puck's y location, but I'm still comparing ideas.

Advantages:

I could get a 3d-like graphical representation with 3d-like controls (3d-like in reality but technically 2d since the lasers are only plotted in the x and y axis though facing the z-axis).

Since this project is going to be my room desk, situated in an automated room, I was thinking of making "desk modes" which should toggle between a game that takes advantage of the lasers and their controls, A control desk for my room, ordinary desk mode, and an air hockey mode.

My question: (More like a request)

Does anyone have another idea regarding how I should be able to locate the puck's x and y location accurately in real time?

EDIT: The table is roll-able and stored underneath a loft bed which has an under-area height of 5'4". Which means I can't go grande on the a vertical solution.

EDIT #2: Thanks to the helpful people here, I have come to the conclusion of using a camera.

The camera will be that of a smartphone's, I'll create an app that tracks an object by color and a has fixed size comparison to identify the distance of the robot from the puck. The phone will then process this and send signals via bluetooth.

The phone is anchored at the end of the robot's moving part so the camera is reminiscent of those games with a first-person view.

Incoming problems: I'm looking forward to some delay, given the delay in processing.


答:
I asked for your code because I've found people tend to implement PID controllers incorrectly. It doesn't really matter what your gains are, if you're not calculating the error terms correctly and/or you're not applying the gains correctly, you'll probably never get the system to work right.

That said, as Arlakt mentioned, your best bet for PID control with no system model is to try different gains. There are several methods for tuning the PID controller, but all of these methods rely on you to calculate the error terms correctly and apply the gains correctly.

:EDIT:

You are not calculating your error terms correctly.

You have:

```<code>error = desired-current;
//error/=390;
integ += error;
der = error - perror;
</code>```

Your integ error term is not an integral and your der error term is not a derivative because you have not incorporated a sample time into your calculation. You need:

```<code>integ += error*sampleTime;
der = (error - perror)/sampleTime;
</code>```

Later, you have:

```<code>v=kpi;x/=100;
v=kii;x2/=1000;
v=kdi;x3/=1000;
</code>```

Which I am assuming is interpreted as:

```<code>v=kpi;
x/=100;
v=kii;
x2/=1000;
v=kdi;
x3/=1000;
</code>```

I do not know why you are dividing your error terms by 100 or 1000, but this is incorrect. I also do not know what v is; it is not a term used anywhere else in the function and is not returned by the function. kpi,kii,kdi are also not defined.

In any event, your entire PID controller should be:

```<code>short PID()
{
short error;
error = desired-current;
integ += error*sampleTime;
der = (error - perror)/sampleTime;
x=error;
x2=integ;
x3=der;
x*=kp;
x2*=ki;
x3*=kd;
x=x+x2+x3;
perror = error;
return x;
}
</code>```

See this post I wrote yesterday on nearly identical flaws in a PID controller. In short, get sampleTime with either a delay/pause or by setting/using a timer.

Title: What PID values should I keep?

Body:
I have built quadcopter but the problem is of balancing. It doesn't goes up. I am using PID technique for balancing. But I am not finding the suitable values for PID tuning. I am using mpu6050 as a sensor. I get the accelerometer values of x and y axis and find the error from them. That is lets say if accel on x is not zero then it error cause it should be zero if balanced. I am using +-2g sensitivity scale of accelerometer. The motors I am using are DJI 920 kva. What values for kp, ki, and kd should I set? I cant set them while in flight cause it completely out of balance.

This is the design. Completely homemade. I have modified it a little after this photo. An accelerometer is at 2g so at balance z will be 32768/2.

```<code>short PID()
{
    short error,v;
    error = desired-current;
    //error/=390;
    integ += error;
    der = error - perror;
    x = error;
    x2 = integ;
    x3 = der;
    x* = kp;
    x2* = ki;
    x3* = kd;
    v = kpi;x/=100;
    v = kii;x2/=1000;
    v = kdi;x3/=1000;
    x = x+x2+x3;
    //x/=390;
    perror = error;
    return x;
}
</code>```

There are also few more questions, should I scale error or PID output? Because, the error is ranging from 0 to 16380 at 2g setting, so I am scaling it from 0 to 42. So should I divide error or PID by some value?

Body:
I asked for your code because I've found people tend to implement PID controllers incorrectly. It doesn't really matter what your gains are, if you're not calculating the error terms correctly and/or you're not applying the gains correctly, you'll probably never get the system to work right.

That said, as Arlakt mentioned, your best bet for PID control with no system model is to try different gains. There are several methods for tuning the PID controller, but all of these methods rely on you to calculate the error terms correctly and apply the gains correctly.

:EDIT:

You are not calculating your error terms correctly.

You have:

error = desired-current;
//error/=390;
integ += error;
der = error - perror;

Your integ error term is not an integral and your der error term is not a derivative because you have not incorporated a sample time into your calculation. You need:

integ += error*sampleTime;
der = (error - perror)/sampleTime;

Later, you have:

v=kpi;x/=100;
v=kii;x2/=1000;
v=kdi;x3/=1000;

Which I am assuming is interpreted as:

v=kpi;
x/=100;
v=kii;
x2/=1000;
v=kdi;
x3/=1000;

I do not know why you are dividing your error terms by 100 or 1000, but this is incorrect. I also do not know what v is; it is not a term used anywhere else in the function and is not returned by the function. kpi,kii,kdi are also not defined.

In any event, your entire PID controller should be:

short PID()
{
short error;
error = desired-current;
integ += error*sampleTime;
der = (error - perror)/sampleTime;
x=error;
x2=integ;
x3=der;
x*=kp;
x2*=ki;
x3*=kd;
x=x+x2+x3;
perror = error;
return x;
}

See this post I wrote yesterday on nearly identical flaws in a PID controller. In short, get sampleTime with either a delay/pause or by setting/using a timer.


问:
Plans to use Vendor ID to identify EtherCAT devices?
I also asked this question on ROS Answers, but it's not getting much interest there.

Currently the EtherCAT package in ROS uses the slaves' Product IDs to identify the devices, and load the correct drivers. This works great when all of the devices are manufactured by a single vendor, but are there any plans to prevent Product ID collisions when multiple vendors make ROS compatible EtherCAT devices?

We manufacture our own EtherCAT devices, and are just using some large values for Product ID, just hoping that these don't collide with anyone else's. Ideally, ROS would concatenate the vendor and product IDs into a single 64-bit value, and use that to identify the correct driver.

答: That certainly is problematic. Given that ROS is free software, you could probably quite easily change the source code to use the <vendor_id, product_code> pair as identifier and send a pull request.


问:
IRobot Create 2: Powering Up after Sleep
I've notice the IRobot Create 2 does not respond to the app's commands when it has been sleeping.  If I press the Clean button and re-run the app then the robot is responsive to the commands.

My initialization sequence (Android/Java) using usb-serial-for-android:

```<code>port.open(connection);
port.setParameters(115200, 8, UsbSerialPort.STOPBITS_1,UsbSerialPort.PARITY_NONE);
command(Opcode.START);
command(Opcode.SAFE);
</code>```

The physical architecture is IRobot Create 2 connected by IRobot Serial Cable to Google Project Tango Tablet.

How can my app wake up the Roomba from it's sleep?

答:
I have been testing Steve’s answer using the Create cable and Python and have ran into some difficulties.

Enabling hardware flow control when opening the serial port results in nothing happening when sending/receiving data from the Create 2.  Page 4 of the guide says “Flow control: None”.  While I have never used hardware flow control, I suspect the Create 2 is not properly handling the handshake so data is not flowing properly.

When enabling hardware flow control did not work at the time of opening the connection, I attempted to make use of the setRTS method.  (Research into this method and setDTR indicate both are implemented in PySerial.)

I tried the following to see if it would wake the Create 2

```<code>serial_port.setRTS(True)
time.sleep(1)
serial_port.setRTS(False)
time.sleep(1)
serial_port.setRTS(True)
</code>```

where the sleep argument of 1 is in seconds.  I also tried the above with the following changes
toggling False, True, then False
setting the sleep period to .5

In addition, I tried all the above with setDTR().  None of the above worked to wake the Create 2.

Next I tried to something simpler – trying to keep the Create 2 from falling asleep.  Based on page 6

“To disable sleep, pulse the BRC pin low periodically before these five minutes expire. Each pulse resets this five minute counter. (One example that would not cause the baud rate to inadvertently change is to pulse the pin low for one second, every minute, but there are other periods and duty cycles that would work, as well.)”

With the Create 2 in Passive mode I tried the following

```<code>while True:
    time.sleep(60)
    serial_port.setRTS(True)
    time.sleep(1)
    serial_port.setRTS(False)
    time.sleep(1)
    serial_port.setRTS(True)
</code>```

This had no effect.  I then tried all the permutations as above when trying to wake the Create 2, but none of these had any effect.  I did find a pdf guide detailing how to make your own Create cable that indicates the BRC line should be connected to the RTS on the computer side, but it does not seem to work.

Body:
I have been testing Steve’s answer using the Create cable and Python and have ran into some difficulties.

Enabling hardware flow control when opening the serial port results in nothing happening when sending/receiving data from the Create 2.  Page 4 of the guide says “Flow control: None”.  While I have never used hardware flow control, I suspect the Create 2 is not properly handling the handshake so data is not flowing properly.

When enabling hardware flow control did not work at the time of opening the connection, I attempted to make use of the setRTS method.  (Research into this method and setDTR indicate both are implemented in PySerial.)

I tried the following to see if it would wake the Create 2

serial_port.setRTS(True)
time.sleep(1)
serial_port.setRTS(False)
time.sleep(1)
serial_port.setRTS(True)

where the sleep argument of 1 is in seconds.  I also tried the above with the following changes
toggling False, True, then False
setting the sleep period to .5

In addition, I tried all the above with setDTR().  None of the above worked to wake the Create 2.

Next I tried to something simpler – trying to keep the Create 2 from falling asleep.  Based on page 6

“To disable sleep, pulse the BRC pin low periodically before these five minutes expire. Each pulse resets this five minute counter. (One example that would not cause the baud rate to inadvertently change is to pulse the pin low for one second, every minute, but there are other periods and duty cycles that would work, as well.)”

With the Create 2 in Passive mode I tried the following

while True:
    time.sleep(60)
    serial_port.setRTS(True)
    time.sleep(1)
    serial_port.setRTS(False)
    time.sleep(1)
    serial_port.setRTS(True)

This had no effect.  I then tried all the permutations as above when trying to wake the Create 2, but none of these had any effect.  I did find a pdf guide detailing how to make your own Create cable that indicates the BRC line should be connected to the RTS on the computer side, but it does not seem to work.


答:
Okay, so developed a flight controller for this project
http://kevinsheridan.info/highly-autonomous-aerial-reconnaissance-robot-proof-of-concept/

I spent 4 months trying to figure out "the right pid values". However in the end it turned out that I was implementing pid incorrectly.

Here is how to implement it. If you are trying to get the quad to stabilize itself at an angle you need to use TWO Pid controllers per axis.

Requested angle & Angle -> PID1 -> rate + requested rate into PID2-> motors

Basically use a pid for rate and for angle feeding into eachother

have a look at my code: http://kevinsheridan.info/highly-autonomous-aerial-reconnaissance-robot-proof-of-concept/haarr-code/low-level-flight-controller-code/

Title: What PID values should I keep?


问:
Magnetic, low insertion force connector
I'm building a robotic tea-maker/watchdog robot and have a power problem. I would like to be able to have the robot approach a socket and insert the power cord of a cheap immersion heater (120V, 300W, see links below) to turn the heater on. However, the force and precision required to plug it into the wall is beyond the capabilities of my stepper motors/Arduino.

My solution was a magnetic breakaway power cord like the charger on a Mac but at higher voltage. Deep fat fryers have suitable ones (120V, high power, see links below). However, the problem is I need both sides of the connector, and I can only find the magnetic breakaway power cord, not the opposite side, which would normally be built into the deep fat fryer. I don't fancy buying a whole fryer just to get one little part...

Any ideas? Alternatives to a breakaway cord? Anyone know of any (cheap) 120V induction chargers? I'll resort to a mechanical on/off switch and just leave the robot plugged in if I have to, but I was hoping for something a bit sleeker.

Links:

Immersion heater

Fryer cord

答:
Since you specifically mention Macs, I'll throw this out there; you can find replacement ports for the actual computer online and it probably wouldn't be too hard to figure out the pinout and wire for 120V service instead, jimmying a MagSafe cable of course being also included. Other than that, I haven't seen too many magnetic breakaway cables like those being produced.

Given that my MacBook Air charger puts out 3A, the 2.5A you require is within limits, although you might want to put a good smattering of epoxy on the terminals to prevent arc-overs.

As to your inductive charging idea, it isn't too hard to build an inductive charging set, although the "isn't too hard" part relies on your immersion heater being of the "dumb" type; that is, a simple resistive heater that is capably powered by DC as easily as it is AC. Here, you'd wire up a primary coil of wire on a "former" for an inductive charging base with some resistance in play to make sure you don't smoke the wire with too high a current. Then, you'd wire the secondary former with a few more turns to account for air-core losses as well as voltage losses due to the primary resistor. Once you had the inductive couple "transformer" wound and drawn up with magnets or what-have-you, you'd bridge-rectify the secondary and install a simple diode clamp or crowbar circuit to limit the voltage across your heater to something safe, in case your inductive couple one day decides to be less lossy than usual.

If you'd like I can hand-draw a schematic for that and post it up; not in a eCAD/EDA mood at the moment.

OwnerDisplayName: IzBelle

Title: Magnetic, low insertion force connector

OwnerDisplayName: ecfedele


问:
Book on mechanisms
I wanted to know if there is any sort of archive of mechanisms that contains a brief description of mechanisms like there type of motion and forces involved. Not lengthy derivations and other stuff. </p>

答: Mechanisms and Mechanical Devices Sourcebook - Nicholas P. Chironis & Neil Sclater, McGraw-Hill, ISBN 0-07-011356-4. The format of this book is more akin to a scrapbook of newspaper clippings, but it is a good visual "food-for-thought" collection spanning several generations of mechanism design for a broad range of applications and industries.</p>

Title: Book on mechanisms


答:
I don't think that you will be able to do this, given the limitations of an IMU. The main limitation being any kind of position or translation data.

With angluar rotations you have the equations shown in the included image (image from here)

From the IMU, you can get angle $\theta$ and you can get angular velocity $\omega$ through some calculations. But to go from there to radius ($r$-this is want you want I believe) or linear velocity $v$ you would need to know arc length $S$. Unfortunately, this is a piece of translation data and that is difficult (at best) to get.

Overall, it seems quite common for users to try and figure out a way to get any kind of positional/translation information from an IMU, but it just isn't the right tool for that job.

EDIT (More details based on comment):

$\theta$ about an axis of rotation is the same no matter how far away (radius $r$) you are from said axis of rotation.

You have two equations available for $\theta$,

$\theta$ = $\omega$ * $t$ (this you can get with the gyro reading and time)

and

$\theta$ = $S$/$r$ (you can directly read $\theta$ from the accelerometers)

Setting those equations equal,

$\omega$ * $t$ = $S$/$r$

Now you only have one equation, but two unknowns. That is not solvable without making an assumption or knowing something else about the system.

Title: Axis of rotation via IMU

Body:
Using an IMU (gyro, accelerometer and magnetometer), as found in most smartphones, can I detect the differences between tilting the device, say forward, along different (parallel) axis positions?

To clarify, if the axis of rotation is far from the sensor, the the motion contains a translational component.

Can the distance and position of this axis be extracted from the IMU data and if so how?

Is there some data fusion algorithm that can do all this?


问:
The relationship between point cloud maps and graph maps
I am most familiar with SLAM maps that are point clouds, usually in the form of a vector like $<x,y,\theta,f_{1x},f_{1y},...,f_{nx},f_{ny}>$.  I also understand how to create a map like this using an EKF.

Today I came across a .graph file format, which as you would expect consists of vertices and edges in the format:

VERTEX2 id x y orientation

EDGE2 observed_vertex_id observing_vertex_id forward sideward rotate inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr

I know that there's a connection between matrices and graphs (an adjacency matrix for example).  But it's not clear to me how this graph format of a map is equivalent to a point cloud map that I'm familiar with.

What is the relationship?  Are the vertices both poses and landmarks? Are they in a global reference frame? How is this created from say velocity information and a range/bearing sensor?  Is there a transformation between a graph map and a point cloud?

答:
As it says in the description of the file format, it is for graph based SLAM approaches. These work on minimizing the error of a pose constraint network. You can think of it this way: There are a number of reference frames (your vertices) and then you have knowledge on the transformation between these frames. These transformations are associated with an uncertainty. Pose graph optimization frameworks like e.g. TORO, HogMan, G2O and so on will then give you the maximum likelihood of your vertex positions, given the constraints.

In practical robot terms, this usually means:

Each robot pose $p_k$ at time $k$ has its own reference frame and hence vertex

Depending on you approach, you can also add landmarks as vertices. You don't have to however.

Whenever you get new information on the relation between two poses, you add that to the constraint graph. E.g. your odometry will give you a transform between $p_k$ and $p_{k+1}$.

If your approach works landmark based, you add transformations to your landmarks. If you only know the position to your landmark, you set a high uncertainty on the rotation information of your transformation.

If your approach does not know about landmarks, e.g. you have large pointclouds that you match with ICP, you can add the ICP results to your constraint graph.

The pose constraints are usuall stored as sparse matrices of size $n \times n$ where $n$ is the number of vertices (again robot poses and landmarks) in your graph.

The file format itself provides initial guesses for the position of the vertices with the VERTEX2 (for 2D models) and VERTEX3 (for 3D models). You can't mix the two.
Constraints are added so that you specify the transform between the reference frames (vertices) given by from_id and to_id. The transform is given by either EDGE2 and EDGE3 as translation and rotation in euler angles, as well as the information matrix of the uncertainty. In this case the information matrix is the inverse of the covariance matrix for the transform vector $[x\, y \, z\, \text{roll}\, \text{pitch}\, \text{yaw}]$.

Depending on your framework, usually one of the vertices is grounded in a global reference frame.

Graph based pose graph optimizers are considered SLAM backends. How you generate the constraints e.g. from you range data is a front-end problem. There is a nice overview in these lecture notes.


答:
When looking at PID loop control code, if statements are always a code smell for me. Every if statement has the potential to add a discontinuity into the control loop that complicates PID tuning immensely. PID loops work much more predictably when their response is smooth (within the bounds of the limits of the system).

Unfortunately, your two main functions, start and setMotors have lots of ifs and thus several potential discontinuities.

setMotors()

If either motor ever gets to max. speed, it is clamped at max. speed without changing the speed of the other motor. If you consider how your robot moves, if one motor is running slower than has been calculated, the other by definition is running faster than it should be for your calculated power difference and you will be moving in the wrong direction. The only way to avoid this is to tune your system so conservatively that you never ask motors to run at maxSpeed.

This isn't helped by the fact that if (mr < -maxSpeed) sets mr = maxspeed rather than mr = -maxspeed which could be rather unfortunate - driving that motor at max. speed in the opposite direction. *8')

Also, if start calculates that a motor should be run in reverse you are throwing away that command and leaving it running at whatever speed it was running before! While start shouldn't do this at the moment, that may not always be the case. If you changed start to better deal with the right angle turn in your track for instance, it could easily require negative speeds for one motor to turn on the spot.

This could have serious repercussions for the stability of your robot.

start()

Another example of a discontinuity is in start, where as the absolute value of power_difference tends towards 0, the both motor speed converges on baseSpeed, but as soon as power_difference hits 0, both motors jump to maxSpeed. Doubling your speed like this can play havoc with with your tuning, and cause you to need to tune much more conservatively to avoid instability.

All it takes is for one motor to respond slightly more aggressively to the command than the other and you have been kicked off in the wrong direction and then your subsequent PID cycles will have to compensate.

Suggestions

I would suggest that you try to rework your calculations so that you have a continuum between maxSpeed through baseSpeed and 0 to -maxSpeed based on the magnitude of the error.

I would also start by making sure that you can follow the track slowly with conservative tuning and then ramp up to higher speeds and more aggressive turns as you get more experienced with running your PID loop.

See also my answer to Line Follower optimization.

Title: Tuning PD for line follower

Body:
I am trying to make line following robot. I am using atmega328p mcu, pololu 10:1 motors, pololu qtr6-rc sensor, 2s li-po. Here is my code:

```<code>/*
* LineFollower.c
*
* Created: 30.04.2015 16:00:05
*  Author: Mikk
*/
#define F_CPU 20000000         //we're running on 20mHz clock

#define numberOfButtons 1

#define READPORT    PORTC
#define READDDR     DDRC
#define READPIN     PINC        // lines connected to PC0 - PC5

#define MAXTICKS    2500
#define QTRCNT      6

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <Mikk/Button.h>
#include <Mikk/QTRRCSensors.h>

int baseSpeed = 70;
int maxSpeed = 140;
const float Kp = 8.1;
const float Kd = 400;

uint8_t mode = 0;                //indicates in which mode program is

uint8_t RmotorSpeed = 0;         //
uint8_t LmotorSpeed = 0;         //motors

void button(void);

void setMotors(int ml, int mr)
{
    if(ml > maxSpeed)             //make sure that speed is not out of range for left motor
        ml = maxSpeed;
    if(ml < -maxSpeed)
        ml = -maxSpeed;

    if(mr > maxSpeed)             //make sure that speed is not out of range for right motor
        mr = maxSpeed;
    if(mr < -maxSpeed)
        mr = maxSpeed;

    if(ml > 0)                    //if left motor speed is positive then drive motor forwards
        LmotorSpeed = ml;
    if(ml == 0)                   //if left motor speed is 0 then stop motor
        LmotorSpeed = 0;

    if(mr > 0)                    //if right motor speed is positive then drive motor forwards
        RmotorSpeed = mr;
    if(mr == 0)                   //if right motor speed is 0 then stop motor
        RmotorSpeed = 0;
}

void emittersOn(void)            //function for turning emitters on
{
    PORTD |= (1 << PIND0);
}

void emittersOff(void)           //function for turning emitters off
{
    PORTD &= ~(1 << PIND0);
}

void LedOn(void)                 //function for turning led on
{
    PORTB |= (1 << PINB5);
}

void LedOff(void)               //function for turning led off
{
    PORTB &= ~(1 << PINB5);
}

void stop(void)                 //stop everything
{
    LedOff();
    setMotors(0, 0);
    emittersOff();
}

void calibration(void)          //calibration takes about 5 seconds
{
    //turn led on
    LedOn();
    //turn emitters on
    emittersOn();
    // reset minimums and maximums
    for (int i = 0; i < QTRCNT; i++)
    {
        QTRmax[i] = 0;
        QTRmin[i] = MAXTICKS;
    }
    //calibrate sensors
    for(int i=0; i<250; i++)
    {
        calibrateQTRs();
        _delay_ms(5);
    }
    //turn emitters off
    emittersOff();
    //turn led off
    LedOff();
}

void start(void)
{
    //turn led on
    LedOn();

    //create all necessary variables
    int power_difference = 0;
    float error = 0;
    float lastError = 0;
    float derivative = 0;
    int position = 0;

    //turn emitters on
    emittersOn();
    _delay_ms(500);               //wait so you can pull your hand away
    while(mode == 2)
    {
        //check for mode change
        button();

        //read position
        position = readLine();
        //make calculations
        error = position - 2500;
        derivative = error - lastError;

        //remember last error
        lastError = error;

        //calculate power_difference of motors
        power_difference = error/(Kp/100) + derivative*(Kd/100);

        //make sure that power difference is in correct range
        if(power_difference > baseSpeed)
            power_difference = baseSpeed;
        if(power_difference < -baseSpeed)
            power_difference = -baseSpeed;

        //drive motors
        if(power_difference > 0)
            setMotors(baseSpeed+power_difference, baseSpeed-power_difference/2);
        else if(power_difference < 0)
            setMotors(baseSpeed+power_difference/2, baseSpeed-power_difference);
        else if(power_difference == 0)
            setMotors(maxSpeed, maxSpeed);
    }
}

void button(void)
{
    char buttonState = 0;
    //check for current button status
    buttonState = ButtonReleased(0, PINB, 1, 200);
    //check if button is pressed
    if(buttonState) //pin change from low to high
    {
        mode++;
        if(mode == 1) calibration();
    }
}

void pwmInit(void)
{
    //set fast-PWM mode, inverting mode for timer0
    TCCR0A |= (1 << COM0A1) | (1 << COM0A0) | (1 << WGM00) | (1 << WGM01) | (1 << COM0B1) | (1 << COM0B0);
    //set fast-PWM mode, inverting mode for timer2
    TCCR2A |= (1 << COM2A1) | (1 << COM2A0) | (1 << WGM20) | (1 << WGM21) | (1 << COM2B1) | (1 << COM2B0);
    //set timer0 overflow interrupt
    TIMSK0 |= (1 << TOIE0);
    //set timer2 overflow interrupt
    TIMSK2 |= (1 << TOIE2);

    //enable global interrupts
    sei();
    //set timer0 prescaling to 8
    TCCR0B |= (1 << CS01);
    //set timer2 prescaling to 8
    TCCR2B |= (1 << CS21);
}

int main(void)
{
    DDRB |= 0x2A;                 //0b00101010
    DDRD |= 0x69;                 //0b01101001
    DDRC |= 0x00;                 //0b00000000

    //clear port d
    PORTD |= 0x00;

    //enable pull-up resistor
    PORTB |= (1 << PINB1);

    initQTRs();
    pwmInit();

    //blink 2 times indicate that we are ready
    for(int i=0; i<4; i++)
    {
        PORTB ^= (1 << PINB5);
        _delay_ms(500);
    }

    while(1)
    {
        button();
        if(mode == 0) stop();
        if(mode == 2) start();
        if(mode >= 3) mode = 0;
    }
}

//update OCRnx values
ISR(TIMER0_OVF_vect)
{
    OCR0A = RmotorSpeed;
}
ISR(TIMER2_OVF_vect)
{
    OCR2A = LmotorSpeed;
}
</code>```

And here is my qtr library:

```<code>    #ifndef QTRRCSensors
#define QTRRCSensors

#define SLOW        1
#define FAST        0

static inline void initQTRs(void)
{
    TCCR1B = (1 << CS11);
}

uint16_t QTRtime[QTRCNT], QTRmax[QTRCNT], QTRmin[QTRCNT];

static inline void readQTRs(uint8_t forceSlow) {
    uint8_t lastPin, i, done = 0;

    for (i = 0; i < QTRCNT; i++)                    // clear out previous times
        QTRtime[i] = 0;

    READDDR |= 0b00111111;                          // set pins to output
    READPORT |= 0b00111111;                         // drive them high

    _delay_us(10);                                  // wait 10us to charge capacitors

    READDDR &= 0b11000000;                          // set pins to input
    READPORT &= 0b11000000;                         // turn off pull-up registers

    TCNT1 = 0;                                      // start 16bit timer at 0
    lastPin = READPIN;

    while ((TCNT1 < MAXTICKS) && ((done < QTRCNT) || forceSlow))     // if forceSlow, always take MAXTICKS time
    {
        if (lastPin != READPIN)                     // if any of the pins changed
        {
            lastPin = READPIN;
            for (i = 0; i < QTRCNT; i++)
            {
                if ((QTRtime[i] == 0) && (!(lastPin & (1<<i))))    // did pin go low for the first time
                {
                    QTRtime[i] = TCNT1;
                    done++;
                }
            }
        }
    }
    if (done < QTRCNT)                              // if we timed out, set any pins that didn't go low to max
        for (i = 0; i < QTRCNT; i++)
            if (QTRtime[i] == 0)
                QTRtime[i] = MAXTICKS;
}

void calibrateQTRs(void) {
    uint8_t i, j;

    for (j = 0; j < 10; j++) {                      // take 10 readings and find min and max values
        readQTRs(SLOW);
        for (i = 0; i < QTRCNT; i++) {
            if (QTRtime[i] > QTRmax[i])
                QTRmax[i] = QTRtime[i];
            if (QTRtime[i] < QTRmin[i])
                QTRmin[i] = QTRtime[i];
        }
    }
}

void readCalibrated(void) {
    uint8_t i;
    uint16_t range;

    readQTRs(FAST);

    for (i = 0; i < QTRCNT; i++) {                  // normalize readings 0-1000 relative to min & max
        if (QTRtime[i] < QTRmin[i])                 // check if reading is within calibrated reading
            QTRtime[i] = 0;
        else if (QTRtime[i] > QTRmax[i])
            QTRtime[i] = 1000;
        else {
            range = QTRmax[i] - QTRmin[i];
            if (!range)                             // avoid div by zero if min & max are equal (broken sensor)
                QTRtime[i] = 0;
            else
                QTRtime[i] = ((int32_t)(QTRtime[i]) - QTRmin[i]) * 1000 / range;
        }
    }
}

uint16_t readLine(void) {
    uint8_t i, onLine = 0;
    uint32_t avg;                                   // weighted total, long before division
    uint16_t sum;                                   // total values (used for division)
    static uint16_t lastValue = 0;                  // assume line is initially all the way left (arbitrary)

    readCalibrated();

    avg = 0;
    sum = 0;

    for (i = 0; i < QTRCNT; i++) {                  // if following white line, set QTRtime[i] = 1000 - QTRtime[i]
        if (QTRtime[i] > 50) {                      // only average in values that are above a noise threshold
            avg += (uint32_t)(QTRtime[i]) * (i * 1000);
            sum += QTRtime[i];
            if (QTRtime[i] > 200)                   // see if we're above the line
                onLine = 1;
        }
    }

    if (!onLine)
    {
        // If it last read to the left of center, return 0.
        if(lastValue < (QTRCNT-1)*1000/2)
            return 0;

        // If it last read to the right of center, return the max.
        else
            return (QTRCNT-1)*1000;

    }

    lastValue = avg/sum;                            // no chance of div by zero since onLine was true

    return lastValue;
}

#endif
</code>```

I am trying to find Kp constant but when it's 7 then my robot just turns off the line always on the same spot. When Kp is 8 then it follows staright line but wobbles a lot and can't take corners. I also tried to increase Kd 10 to 20 times when my Kp was 8 but it didn't change much. How can I get it working?

Here is my robot and the track I want to follow.