Working Code

I finished fixing all the problems I was having with the code, which boiled down to the use of using  memory on the stack and using c string functions. I’m doing a couple of over-the-top things in my code that I am hoping will make it very easy to specify exact angles, distances, and times in an intuitive way. The following uses advanced programming concepts.

Intuitive Commands

I wanted to avoid “hard coding” commands, because it would be very difficult for anyone who doesn’t know the code to change anything. Instead I abstracted commands to a text string. Here’s an example:

char* commands = "forward 10,stop 300,left 90,stop 300,repeat";

It should be immediately obvious what this does. The bot goes forward 10 inches, then stops for 300 milliseconds, then turns left 90 degrees, etc. It then repeats the sequence.

To process this string, I am splitting the string on commas, storing each command in an array of strings called commandList. I use an iterator for commandList, incremented when I get a new command. When I process a command, I split the command on spaces to get the name and value, then depending on the name, I assign the value and change state. While in that state, I will test for a condition repeatedly.

So for example, I read “forward 10”, so I set “distance” to 10 and change state to STATE_FWD. While in this state, I will calculate the velocity, calculate the distance traveled from that, get the remaining distance, then estimate the remaining time. Then test if the remaining time has reached zero, at which point the state ends.

Improved Interrupts

As I stated before, timing will not be ideal by doing the following method and I would rather use timing interrupts, but I’ll need to use AVR C for that. Regardless I wanted to get something finished in Arduino first, and this is what was the result.

Arduino has two external interrupts. I’m using them for encoders. In my previous blog entry about interrupts, I said to attach and detach interrupts in commands, but that isn’t required. Here is the new code.

void addLeft()
{
 //check time, skip interrupt if one just happened within time fix
 unsigned long thisTick = millis();
 if(thisTick < leftLastTick) return;
 leftLastTick = thisTick + fix;
 leftTicked = true;
}
void goForward()
{
 digitalWrite(pin_motor_left[0], HIGH);
 digitalWrite(pin_motor_left[1], LOW);
 digitalWrite(pin_motor_right[0], HIGH);
 digitalWrite(pin_motor_right[1], LOW);
 encDirLeft = 1;
 encDirRight = 1;
}

When there is an interrupt, we immediately want to get the time, ignore if it has happened too soon, else we set a 50ms delay and set a flag, indicating the occurrence of an interrupt. Now, because we have 50ms before the next interrupt, I am entirely assuming that the loop() function will pass at least a few times in that time.

Calculating Velocity

In the loop, I have a readSensors() function where I call readEncoders() where I read the flags set by the encoders. If they are set, I then get use the time of the interrupts to calculate velocity. There are 10 ticks per rotation, the circumference is 8.05″ meaning 0.805″ between ticks. v = dx/dt (change in velocity over change in time) gives velocity. I then store the current velocity in an array. That is enough calculation for now.

When I want the actual velocity, I call a function calcV() that takes the average of the last 5 velocities. I only do this once per loop and I will use it whenever I do a status update or I am calculating the time remaining in a dependent state.

void readEncoders()
{
 if (leftTicked)
 {
 leftTicked = false;
 encTicksLeft++;
 leftTicks[iL] = leftLastTick;
 if (encTicksLeft < 2)
 {
 leftVs[iL] = 0; //cant calc v yet
 }
 else
 {
 //calc v = dx/dT
 //dx = distance of 1 tick, or 1/10 of wheel rot
 //dT in ms
 //v = ticks per ms
 leftVs[iL] = 1.0 / (leftTicks[iL] - leftTicks[(iL+4)%5]);
 }
 iL = (iL+1)%5;
 }
}
void calcAvgV()
{
 if (done) return;
 done = true;
 //get average vL
 int countL = encTicksLeft>5 ? 5 : encTicksLeft; //min(x,y)
 int e = (iL+4)%5;
 for(int j=iL;j!=e;j=(j+1)%5)
 {
 if (leftVs[j] < 0) continue;
 vLeft += leftVs[j];
 if(j==e) break;
 //a = dV/dT
 aLeft += (leftVs[j] - leftVs[(j-1)%5])/(leftTicks[j]-leftTicks[(j-1)%5]);
 }
 //ticks per ms
 vLeft = countL>0 ? vLeft / countL : 0;
 aLeft = countL>1 ? aLeft / (countL - 1) : 0;
//get average vR
// ... (similar to above)
//
//calc total vectors
 //ticks per ms
 v = (vLeft * encDirLeft + vRight * encDirRight) / 2;
 //inches per ms
 v = v * TDIST;
}

And of course if you want to calculate angular velocity, you take the standard formula of taking the tangential velocity divided by radius. See Game Physics Part 2. The radius being the point at the center of a wheel to the center of the bot.

void calcAngV()
{
 //clockwise = positive
 //vLeft in ticks per ms
 //times the direction
 //times cos 45
 //times the tick distance
 //angV = (inches per ms) / (distance from center of side to center of bot)
 calcAvgV();
 angV = ((vLeft * encDirLeft + vRight * encDirRight * -1) * COS45 * TDIST) / WDIST;
 angV *= 10;
}
Advertisements

Advanced C in Arduino

I have reached the limits of what I can do with the arduino API. I started using c language functions. And later on I may code in AVR C directly.

Stuff you should be aware of when using C:
  • You must allocate memory for any variables you use or they may be overwritten when the program goes out of scope where the variable was created. For example: a function creates an int x inside the function and returns it. Then a printf statement mysteriously changes x to some wierd characters. It’s easiest to just use global variables for all variables used anywhere.
  • Arduino’s Serial doesn’t provide string formatting so the output can very easily be a complete mess. Also, Atmel AVR doesn’t allow floats in sprintf or any other c-string functions. It’s possible to work around this by just multiplying numbers by 100 and using them as integers, then use string manipulation to put a period at 2 decimals. Example:  sprintf(“%3d.%02d”,12,13) gives the string ” 12.13″.
  • The above point is more complicated than it sounds, and you need to use proper c string functions like strcpy(str1,str2) instead of str1 = str2. And these functions require character arrays e.x. char str [40]; as parameters, but they usually require string pointers as a return type e.x. char* str; These are completely different types.
  • In my code I have used the strtok() function, which splits a string on delimiters,  and found that you need to use a hard copy of the string you are parsing because it changes the string.
  • Pretty much you need to know exactly what a function does and how it uses its parameters before you implement it or you will be doing alot of debugging.

Arduino Code – Status

Displaying Status

Just a short post to talk about displaying output from the Arduino. You will want it to display time stamps, each sensor, and the state. The display should be easy to read and easy to compare between each update. You will want to update slow enough to read it, but fast enough to get relevant data.

The human eye can see up to about 30 FPS (frames per second), and anything past 60 FPS is impossible to see. If we want to be able to read anything, it should be at most 15 FPS. We will want to create function called Status() to display relevant data. We will want to add a call to a draw() function at the end of the loop() function to display stuff in the Serial at some specified FPS.

int FPS = 15; //frames per second
double fFPS;

//...

void setup()
{
  //...

  fFPS = (1.0 / FPS) * 1000 - 1;
}
}
void loop()
{
  //...

  draw();
}
void draw()
{
  //This function is used for displaying status and other future gui stuff
  //at a given FPS
  frameTime = millis() - frameTimeStart;
  if(frameTime >= fFPS)
  {
    frameTimeStart = millis();
    frameTime = 0;
    Status();
  }
}
void Status()
{
  unsigned long ms = millis();
  unsigned long s = ms/1000;
  unsigned long m = s/60;
  unsigned long h = m/60; 
  //format for hh:mm:ss:000
  ms = ms % 1000;
  s = s % 60;
  m = m % 60;
  String str = String(h) + ":" + String(m)+":" + String(s) + ":" + String(ms) + 
            " \tS:" + String(state) +
            "\tENC-L: " + String(encTicksLeft) + "\tENC-R: " + String(encTicksRight) +
            "\tIR-L:" + String(read_IR_left) + "  \tIR-R:" + String(read_IR_right) +
            "  \tSnd: " + String(read_sonar);
  Serial.println(str);
}

Sample Output

This is sample output as the robot approaches a wall and stops before it. Notice the ultrasonic sound sensor decreasing as it gets closer to the wall and notice the encoders ticking up at the same rate. As the state changes, the encoders are reset.

0:0:2:246 	S:1	ENC-L: 0	ENC-R: 0	IR-L:204  	IR-R:188  	Snd: 64
0:0:2:328 	S:1	ENC-L: 0	ENC-R: 0	IR-L:207  	IR-R:188  	Snd: 64
0:0:2:411 	S:1	ENC-L: 0	ENC-R: 0	IR-L:210  	IR-R:187  	Snd: 64
0:0:2:493 	S:1	ENC-L: 0	ENC-R: 0	IR-L:208  	IR-R:188  	Snd: 64
0:0:2:576 	S:1	ENC-L: 1	ENC-R: 1	IR-L:220  	IR-R:246  	Snd: 64
0:0:2:658 	S:1	ENC-L: 2	ENC-R: 2	IR-L:140  	IR-R:115  	Snd: 64
0:0:2:741 	S:1	ENC-L: 4	ENC-R: 4	IR-L:64  	IR-R:23  	Snd: 63
0:0:2:821 	S:1	ENC-L: 5	ENC-R: 5	IR-L:41  	IR-R:4  	Snd: 62
0:0:2:900 	S:1	ENC-L: 7	ENC-R: 7	IR-L:31  	IR-R:3  	Snd: 61
0:0:2:979 	S:1	ENC-L: 8	ENC-R: 8	IR-L:27  	IR-R:6  	Snd: 59
0:0:3:58 	S:1	ENC-L: 10	ENC-R: 10	IR-L:27  	IR-R:5  	Snd: 57
0:0:3:139 	S:1	ENC-L: 11	ENC-R: 11	IR-L:25  	IR-R:6  	Snd: 55
0:0:3:220 	S:1	ENC-L: 13	ENC-R: 13	IR-L:31  	IR-R:13  	Snd: 54
0:0:3:302 	S:1	ENC-L: 14	ENC-R: 14	IR-L:40  	IR-R:25  	Snd: 52
0:0:3:385 	S:1	ENC-L: 16	ENC-R: 16	IR-L:31  	IR-R:6  	Snd: 50
0:0:3:466 	S:1	ENC-L: 17	ENC-R: 17	IR-L:28  	IR-R:3  	Snd: 48
0:0:3:548 	S:1	ENC-L: 19	ENC-R: 19	IR-L:25  	IR-R:3  	Snd: 46
0:0:3:629 	S:1	ENC-L: 20	ENC-R: 20	IR-L:24  	IR-R:6  	Snd: 44
0:0:3:709 	S:1	ENC-L: 22	ENC-R: 22	IR-L:24  	IR-R:7  	Snd: 42
0:0:3:791 	S:1	ENC-L: 24	ENC-R: 24	IR-L:25  	IR-R:3  	Snd: 40
0:0:3:872 	S:1	ENC-L: 25	ENC-R: 25	IR-L:39  	IR-R:4  	Snd: 38
0:0:3:954 	S:1	ENC-L: 27	ENC-R: 26	IR-L:23  	IR-R:15  	Snd: 36
0:0:4:36 	S:1	ENC-L: 28	ENC-R: 28	IR-L:25  	IR-R:0  	Snd: 34
0:0:4:116 	S:1	ENC-L: 30	ENC-R: 30	IR-L:25  	IR-R:4  	Snd: 32
0:0:4:198 	S:1	ENC-L: 31	ENC-R: 31	IR-L:26  	IR-R:3  	Snd: 30
0:0:4:279 	S:1	ENC-L: 33	ENC-R: 33	IR-L:25  	IR-R:5  	Snd: 28
0:0:4:360 	S:1	ENC-L: 35	ENC-R: 34	IR-L:36  	IR-R:17  	Snd: 26
0:0:4:443 	S:1	ENC-L: 36	ENC-R: 36	IR-L:38  	IR-R:21  	Snd: 24
0:0:4:525 	S:1	ENC-L: 38	ENC-R: 38	IR-L:43  	IR-R:4  	Snd: 22
0:0:4:606 	S:1	ENC-L: 39	ENC-R: 39	IR-L:26  	IR-R:22  	Snd: 20
0:0:4:688 	S:2	ENC-L: 41	ENC-R: 41	IR-L:42  	IR-R:41  	Snd: 18
0:0:4:771 	S:2	ENC-L: 43	ENC-R: 42	IR-L:75  	IR-R:103  	Snd: 17
0:0:4:854 	S:3	ENC-L: 0	ENC-R: 0	IR-L:97  	IR-R:95  	Snd: 17
0:0:4:934 	S:3	ENC-L: 0	ENC-R: 0	IR-L:118  	IR-R:102  	Snd: 19
0:0:5:17 	S:3	ENC-L: 0	ENC-R: 0	IR-L:101  	IR-R:77  	Snd: 19
0:0:5:97 	S:3	ENC-L: 0	ENC-R: 0	IR-L:103  	IR-R:78  	Snd: 19

Arduino code

If you have read my previous post about encoders and have seen the video of our bot completing a square you must be wondering how we are guiding the robot using encoder ticks. We are not using any delay() function calls anywhere. It happens that the loop() acts similar to the Update() function in an XNA game, therefore it makes perfect sense to use the same concepts I have used to make XNA games. I’m going to explain some of the essential concepts needed for efficient and easy control of the robot. I’ll be using some advanced programming concepts.

State Machines

Philosophy

To define the behavior of an object within an environment, the object must follow rules set by the environment. Within an environment, we seek to master our understanding of the rules in order to accomplish alternative goals with relative ease. In this case, our rules are simply two spacial dimensions and time. One task is to master precise movements with the robot on the ground, and the other is to master precise timing of those actions. The latter can be done easily with a state machine. And of course our alternative goal is to draw a picture on the ground.

States

What If I want the robot to traverse a square? You would give the bot a series of commands to follow. Each command acts as an event that puts the robot into a new state, where the bot acts a certain way until another event occurs that puts it in a different state where it acts a different way.

This diagram shows the basic concept of switching states. It describes a repeating square pattern. In the code I have twice as many states, because I only want to give it a command once per state.

void loop()
{
  stateTime = millis() - stateTimeStart;
  //state machine
  switch (state)
  {
    case STATE_FWD_BEGIN:
      changeState(STATE_FWD);
      goForward();
      break;
    case STATE_FWD:
      if(encTicksLeft >= 40)
      {
        Stop();
        changeState(STATE_FWD_END);
      }
      break;
    case STATE_FWD_END:
      if(stateTime >= 500) changeState(STATE_LEFT_BEGIN);
      break;
    case STATE_LEFT_BEGIN:
      turnLeft();
      changeState(STATE_LEFT);
      break;
    case STATE_LEFT:
      if (encTicksRight >= 26)
      {
        Stop();
        changeState(STATE_LEFT_REV);
      }
      break;
    case STATE_LEFT_END:
      if(stateTime >= 500) changeState(STATE_FWD_BEGIN);
      break;
  }
}
void changeState(int s)
{ 
  state = s;
  stateTime = 0;
  stateTimeStart = millis();
}

Note that the loop function is called repeatedly until the bot is turned off, and there are no delays, which means the switch statement is iterated through hundreds of times a second. Each of the STATE_ variables are unique integers counting from 0. Switch statements are far more efficient than a series of if statements, especially when the cases are enumerated using integers, because of the way the compiler is implemented; the cases are jumped to instantly, instead of comparing each value until a match is found.

Note that instead of using calls to the delay() function, I explicitly keep track of the time spent in a state.

Philosophy

With a state machine we have an acceptable control over time when we want to determine to a good estimate when something needs to happen. This loop may be cycling over 200 times a second, but that still isn’t precise enough for the encoders. Our code may grow in size the more we fiddle with it, but we don’t want that to affect our sensor readings. In this case the only choice is to continue to use interrupts.

Now that we have complete domination of time, we still have to deal with space, which is a more compound problem, because accurate measurements of space will require precise measurements of time.

Encoders

Encoders

Encoders are sensors attached on the inside of one of the motors on each side to measure their rotation, allowing us to get the distance and velocity of the robot. They use a little fixated laser light that passes through a notched wheel attached to the wheel axel. When the motor spins, the light is blocked and cleared 10 times every full wheel rotation.

 Using Encoders

We could assign a variable to read the values given by the encoder, which gives a value HIGH or LOW indicating whether the light is blocked or not. We could run this code in the loop() function, reading the value of the encoder every so often and we could count every time the light is blocked. However, doing so is not viable, because we would have to wait for that block of code to iterate again and we want to know exactly when the encoder value changes.

Interrupts

We can get the exact time an encoder changes value by attaching an interrupt to each encoder. An interrupt is an ability a microprocessor uses to prioritize code. The Arduino Uno comes with 2 interrupts, 0 and 1. An interrupt should perform only a very short code segment and return to the previous code.

When we are interested in data from the encoder, we attach an interrupt and a function to an encoder. Detach the interrupt when it stops, because we will need to use different interrupts for other commands.

//interrupts
void addLeft()
{
 encTicksLeft++;
}
void addRight()
{
 encTicksRight++;
}
//commands
void goForward()
{
 attachInterrupt(0, addLeft, RISING);
 attachInterrupt(1, addRight, RISING);
  digitalWrite(pin_motor_left[0], HIGH);
  digitalWrite(pin_motor_left[1], LOW);
  digitalWrite(pin_motor_right[0], HIGH);
  digitalWrite(pin_motor_right[1], LOW);
 encTicksLeft = 0;
 encTicksRight = 0;
}
void Stop()
{
 detachInterrupt(0);
 detachInterrupt(1);
  digitalWrite(pin_motor_left[0], LOW);
  digitalWrite(pin_motor_left[1], LOW);
  digitalWrite(pin_motor_right[0], LOW);
  digitalWrite(pin_motor_right[1], LOW);
 encTicksLeft = 0;
 encTicksRight = 0;
}

Encoder Caveats

We experienced some very strange problems with implementing our encoders, though resolved. We noticed that when a wheel is given high acceleration, the pulse given by the encoder will react by jerking up and down rapidly, giving a large amount of interrupts in a short time frame. We were seeing jumps of 100 interrupts in a matter of 50 milliseconds, that would be 10 wheel rotations in 1/20 of a second, or given the wheels are 2.56″ wide, the circumference = 2.56 * pi = 8.05″ divide by 1/20 s = 161 in/s /12 = 13.42 feet per second. Yeah, I don’t think so.

In fact, jerking one of the motors like this would even have an effect on the other encoder causing 10+ interrupts on the other motor, even though it hadn’t moved at all.

There are two ways to fix this problem. It can be done by physically adding a double nand gate to smooth out the pulse, which I think may be necessary to fix the second issue I mentioned, but I’ll have to let the rest of my group figure that out.

Another way we came up with was to ignore interrupts from an encoder for a short period of time after an interrupt had occurred. The following code adds a delay before the next interrupt is accepted.

//interrupts
volatile unsigned long lastInterrupt = 0; //time of last interrupt
const int fix = 50; //after an interrupt, don't interrupt again for this long (ms)
void addLeft()
{
  //check time, skip interrupt if one just happened within time fix
 unsigned long thisInterrupt = millis();
 if(thisInterrupt < lastInterrupt) return;
 lastInterrupt = thisInterrupt + fix;
  encTicksLeft++;
}

millis() returns the time in milliseconds since the program started. The fix variable had to be short enough that it doesn’t ignore the next real tick, but long enough that it ignores all the fake ticks. We found that 20 ms was too short, and 50 ms seems to work fine.

With a little math we can figure out with a good estimate how long it should be. We did a short test where the bot moved 0.915 f/s. The wheels are 2.5625 inches in diameter  = 2.565pi = 8.05 inches circumference. 0.915 f/s * 12 = 10.982 in /s / 8.05 =  1.3642 rot/s * 10 = 13.642 ticks/s / 1000 =.013642 ticks/ms. 1/.013642 = 73.3 ms / tick. Or about every 73.3 ms there is an interrupt. If the bot was moving at 2 f/s, it works out to about 33.6 ms between ticks.

This means that a 50ms delay after an interrupt before accepting another interrupt is just fine given our bot’s speed, but if it had only double the speed, the 50ms delay becomes too much and it will start skipping interrupts.

Before you use the above code, there’s one more caveat. I noticed when I was testing the code that one wheel was sometimes only interrupting half as often as the other. It happens that when both wheels interrupt at the same time within that 50 ms time frame, only the first wheel gets an interrupt and it ignores the second wheel.

We had to use separate delays for the interrupts:

//interrupts
volatile unsigned long leftLastInterrupt = 0; //time of last interrupt
volatile unsigned long rightLastInterrupt = 0; //time of last interrupt
const int fix = 50; //after an interrupt, don't interrupt again for this long (ms)
void addLeft()
{
  //check time, skip interrupt if one just happened within time fix
  unsigned long thisInterrupt = millis();
  if(thisInterrupt < leftLastInterrupt) return;
  leftLastInterrupt = thisInterrupt + fix;
  encTicksLeft++;
}
void addRight()
{
  //check time, skip interrupt if one just happened within time fix
  unsigned long thisInterrupt = millis();
  if(thisInterrupt < rightLastInterrupt) return;
  rightLastInterrupt = thisInterrupt + fix;
  encTicksRight++;
}

Arduino

Arduino – Read more about it here

Our robot uses the Arduino Uno chipset to control all of the motors and sensors. We are prototyping everything on the board with wires, a breadboard “H-Bridge” and the arduino software. Later on we will solder the wires directly to the board and we will use a lower level programming language: Atmel AVR-C. But first, an update. This post involves basic programming concepts.

The basic Arduino program

Every arduino program has 2 essential functions, setup() and loop(). The setup(0 function is called once as it starts, then the loop() function is called repeatedly until it is turned off.

void setup() {
// ...
}
void loop() {
// ...
}

I will be using 4 motors, however the board only has so many pins. That’s ok though because I can just control the left side motors and the right side motors together as if they were tank treads. To control the motors on one side, I need 2 pins to control the current of electricity going to them.

  const int pin_motor_left[] = {4, 5};
  const int pin_motor_right[] = {9, 6};
  digitalWrite(pin_motor_left[0], HIGH);
  digitalWrite(pin_motor_left[1], LOW);
  digitalWrite(pin_motor_right[0], HIGH);
  digitalWrite(pin_motor_right[1], LOW);

In this code segment I have arrays holding the pin numbers for the polarities of each side. I use digitalWrite to direct the current going to a pin. The left motors are connected to pin 4 and 5, so if I wrote a HIGH to 4 and LOW to 5, it will cause the motor to spin forward. If I wrote LOW, LOW, the motor would stop, and if I wrote LOW, HIGH, the motor would spin backward.

I’ll put these digitalWrites into functions for ease called goForward(), Stop(), turnLeft(), turnRight(), goBackward(). The loop function happens to iterate something on the order of a thousand times a second if there isn’t anything for it to do. We can add a delay() (in milliseconds) after a function call. This is the code so far:

//Output Pins
const int pin_motor_left[] = {4, 5};
const int pin_motor_right[] = {9, 6};
void setup()
{
  //motors
  pinMode(pin_motor_left[0], OUTPUT);
  pinMode(pin_motor_left[1], OUTPUT);
  pinMode(pin_motor_right[0], OUTPUT);
  pinMode(pin_motor_right[1], OUTPUT);
}
void loop()
{
  //makes some kind of polygon
  goForward();
  delay(1000);
  Stop();
  delay(500);
  turnLeft();
  delay(1000);
  Stop();
  delay(500);
}
void goForward()
{
  digitalWrite(pin_motor_left[0], HIGH);
  digitalWrite(pin_motor_left[1], LOW);
  digitalWrite(pin_motor_right[0], HIGH);
  digitalWrite(pin_motor_right[1], LOW);
}
void turnLeft()
{
  digitalWrite(pin_motor_left[0], LOW);
  digitalWrite(pin_motor_left[1], HIGH);
  digitalWrite(pin_motor_right[0], HIGH);
  digitalWrite(pin_motor_right[1], LOW);
}
void Stop()
{
  digitalWrite(pin_motor_left[0], LOW);
  digitalWrite(pin_motor_left[1], LOW);
  digitalWrite(pin_motor_right[0], LOW);
  digitalWrite(pin_motor_right[1], LOW);
}

Notice in setup() you need to specify the motor pins as OUTPUT. Later I’ll use sensors on pins with INPUT. Look at the loop function. It should be easy to tell what it does: moves forward for 1 second (1000 ms), stops for half a second, turns left for 1s, stops and repeats that forever.

When we ran the bot with this setup it was immediately evident that it was just about completely random where the robot would face next, because it takes time for the robot to accelerate and stop, and the wheels slip when it turns. It was also difficult to specify the angle we wanted the robot to face, because it’s based on time. We will certainly need near perfect angles to make a dodecahedron, which we will be able to do with encoders.

Game Physics – Part 2

If you haven’t yet, read Game Physics Part 1

Implementing Wheel Commands

The next step I want to take this little simulation program is to bring the top-down development down another level. From the previous post, we have outlined the physics needed to accurately describe the movement of objects in a 2d field. The objects have two basic control vectors: a forward distance vector and a rotational vector. However, the robots we will be using in real life will use four distinct control points, namely the four wheels.

While we will only be implementing control of each side instead of each wheel, it will still be important to include individual wheel control in the simulation, because it is possible that each motor may produce a different speed for whatever reason and we can induce those errors into the simulation.

Take a look at the following diagram and I’ll explain it afterwards :

So here is a stripped version of the robot from a bird’s perspective. The green arrows on each wheel represent the velocity projected by the wheels. The top wheels are moving the bot left, and the bottom wheels are moving the bot right. This should make the robot spin in place. Notice the green circle intersects the center of every wheel. The pivot point for the robot should be the center of the robot. The dark blue line intersects the center of the bot with a wheel.

I want to translate the green vectors on each wheel into the green vector at the center of the bot, and the light blue rotational scalar. To do this, we use the dark blue line as the axis and break down the green wheel vectors into component vectors. These are the pink and yellow arrows. Notice the pink arrow is perpendicular, and the yellow line is parallel to the dark blue line. Notice the pink arrow is tangent to the circle.

To calculate the rotation of the bot, we add up the magnitude of each pink vector. This gives us a scalar, undirected value we can use for the rotation of the bot. Note that we also need to indicate whether the rotation is clockwise or counter-clockwise, which we can do with negative or positive. The yellow vector is perpendicular to the circle, having no effect on the rotation, and is therefore unused.

To calculate the forward vector, we can take the average vector of every wheel. Now we have all the components we need and can start programming.

https://cneuburg.wordpress.com/2011/10/22/game-physics-part-1/