Hi, I am making a robot that corrects for wheel slippage by using a gyroscope and wheel encoders. I will post the details later.
Basically, I am able to read the encoder, gyroscope, and make the robot move forward. What I want to do is be able to take readings from the gyro and encoders in the background while the robot is moving around.
Does anyone have any ideas?
I've heard that ATmel uses "tables" as a sort of "state diagram" but I'm not quite sure what that means.
NOTE: the code below does not include all the methods. namely it is missing enc1a(), enc1b(), enc1c(), enc1d(), enc2a() . . .
The gyro uses SPI interface and the motorcontroller uses I2C
/Users/Ross/Desktop/robot.jpg
#include <Behavior.h>
#include <Wire.h>
#include <math.h>
#define comb4bytes(a,b,c,d) ( ((a) << 24) | ((b) << 16) | ((c) << 8) | (d) ) // combines two bytes into a word
#define RSHFT(x) (x >> 1) // gets rid of the LSB
#define SCLK 13
#define SS 10
#define MOSI 11
#define MISO 12
#define UBLB(a,b) ( ( (a) << 8) | (b) ) // combines two bytes into a word
// the commands needed for the MD23 sensor:
#define sensorAddress (0x58) // 0xB0 right shifted by 1 bit
const float pi = 3.14159;
void resetEncs()
{
Wire.beginTransmission(88); // transmit to device #112
Wire.send(16); // sets register pointer to echo #1 register (0x02)
Wire.send(32); // command register sets encoder to zero (0x20)
Wire.endTransmission();
}
void setup()
{
Wire.begin(); // join i2c bus (address optional for master)
Serial.begin(9600); // start serial communication at 9600bps
pinMode(SCLK, OUTPUT); // sets the digital pin as output
pinMode(SS, OUTPUT);
pinMode(MOSI, OUTPUT);
pinMode(MISO, INPUT);
digitalWrite(SS, HIGH); // disables the device
byte clr;
SPCR = (1<<SPE)|(1<<MSTR); // clear SPDR & SPCR
clr = SPSR;
clr = SPDR;
resetEncs(); // reset encoder to zero
Serial.println("Setup Done . . . ");
}
/*
strait() sends commands in the format the MD23 sensor expects
*/
void strait()
{
Wire.beginTransmission(sensorAddress);
Wire.send(0);
Wire.send(60);
Wire.send(60);
Wire.endTransmission();
}
/*
rturn() sends commands in the format the MD23 sensor expects
*/
void stop()
{
Wire.beginTransmission(sensorAddress);
Wire.send(0x00);
Wire.send(-128);
Wire.send(-128);
Wire.endTransmission();
}
void rturn(int type)
{
// start I2C transmission:
Wire.beginTransmission(sensorAddress);
// send command:
Wire.send(0x00);
Wire.send(-128); // type of turn: about outside wheel or about center axis
Wire.send(60);
Wire.endTransmission();
}
void lturn(int type)
{
// start I2C transmission:
Wire.beginTransmission(sensorAddress);
// send command:
Wire.send(0x00);
Wire.send(60);
Wire.send(-128); // type of turn: about outside wheel or about center axis
Wire.endTransmission();
}
char spi_transfer(volatile char outgoing_byte)
{
SPDR = outgoing_byte;
while (!(SPSR & (1<<SPIF)))
{
};
return (SPDR); // return the received byte
}
int deg = 0;
int rev = 0;
int temp;
byte data1, data2;
byte bit7 = B10000000;
byte bit5 = B00100000;
int accel;
int ADCcode;
void readGyro()
{
digitalWrite(SS,LOW);
spi_transfer(148);
digitalWrite(SS, HIGH);
delay(250);
digitalWrite(SS,LOW);
spi_transfer(128);
data1 = spi_transfer(0); // data1 = B00100111;
data2 = spi_transfer(0);
Serial.print("\n");
digitalWrite(SS,HIGH);
if (((bit7 & data1) == 0) && ((bit5 & data1) == 32))
{
temp = UBLB(data1, data2); // combines two bytes into a word
//Serial.println(temp, BIN);
temp = RSHFT(temp);
ADCcode = ((temp & 2047));
accel = (((25/12)*ADCcode + 400) - 2400)*75; //
Serial.print("Rate = ");
Serial.print(accel/1000);
Serial.print(" deg/sec\n");
}
}
float deltaTheta1 = 0;
int readEnc1()
{
// step 1: instruct sensor to read echoes
//
Wire.beginTransmission(88); // transmit to device #112 (0x58)
// the address specified in the datasheet is 176 (0xB0)
// but i2c adressing uses the high 7 bits so it's 88
Wire.send(0x00); // sets register pointer to the command register (0x00)
Wire.send(-128); // command sensor to measure in "inches" (0x50)
Wire.send(-120); // use 0x51 for centimeters
// use 0x52 for ping microseconds
Wire.endTransmission(); // stop transmitting
// step 2: wait for readings to happen
delay(70); // datasheet suggests at least 65 milliseconds
enc1d();
enc1c();
enc1b();
enc1a();
deltaTheta1 = temp3;
Serial.print("DeltaTheta1 = ");
Serial.print(deltaTheta1, DEC);Serial.print("/100 radians");
Serial.print("\n");
delay(400);
return deltaTheta1;
}
float deltaTheta2 = 0;
int readEnc2()
{
// step 1: instruct sensor to read echoes
//
Wire.beginTransmission(88); // transmit to device #112 (0x58)
// the address specified in the datasheet is 176 (0xB0)
// but i2c adressing uses the high 7 bits so it's 88
Wire.send(0x00); // sets register pointer to the command register (0x00)
Wire.send(-128); // command sensor to measure in "inches" (0x50)
Wire.send(-120); // use 0x51 for centimeters
// use 0x52 for ping microseconds
Wire.endTransmission(); // stop transmitting
// step 2: wait for readings to happen
delay(70); // datasheet suggests at least 65 milliseconds
enc2d();
enc2c();
enc2b();
enc2a();
deltaTheta2 = temp7;
Serial.print("DeltaTheta2 = ");
Serial.print(deltaTheta2, DEC);Serial.print("/100 radians");
Serial.print("\n");
delay(400);
return deltaTheta2;
}
int Width = 35; // width of wheel base is 35 cm
int radius = 5; //radius of the wheels in cm
float xCoordinate = 0;
int xLocation(float tempPhi,float xThetaR, float xThetaL)
{
//Serial.print("prevPhi = ");
// Serial.print(tempPhi);
// Serial.print("\n");
xCoordinate = xCoordinate - sin(tempPhi)*((xThetaR + xThetaL)/(2))*(Width);
Serial.print("x = ");
Serial.print(xCoordinate, DEC);//Serial.print(" cm ");
Serial.print("\n");
}
int yLocation(float tempPhi,float yThetaR, float yThetaL)
{
int yCoordinate = 0;
yCoordinate = yCoordinate + cos(tempPhi)*((yThetaR + yThetaL)/(2))*(Width);
Serial.print("y = ");
Serial.print(yCoordinate);Serial.print(" cm ");
Serial.print("\n");
}
float Phi = 0;
float prevPhi = 0;
int instPhi = 0;
int deltaPhi(int ThetaR, int ThetaL)
{
/* Serial.print("ThetaR = ");
Serial.print(ThetaR);
Serial.print("\n");
Serial.print("ThetaL = ");
Serial.print(ThetaL);
Serial.print("\n"); */
prevPhi = Phi;
Phi = Phi + (ThetaR - ThetaL)/(7);
Serial.print("Phi = ");
Serial.print(Phi, DEC);Serial.print(" radians");
Serial.print("\n");
instPhi = (Phi - prevPhi)*100;
// float y = 1.57;
int x = 100*sin(instPhi/100);
Serial.print("instPhi = ");
Serial.print(instPhi);
Serial.print("\n");
Serial.print("x = ");
Serial.print(x);
Serial.print("\n");
xLocation(instPhi, ThetaR, ThetaL);
yLocation(instPhi, ThetaR, ThetaL);
return Phi;
}
void loop()
{
/* float y = 1.57;
int x = 100*sin(y);
Serial.print("y = ");
Serial.print(x);
Serial.print("\n"); */
// RunAllReady();
//Behavior(strait());
//Behavior(readGyro());
//delay(2000);
//readEnc1();
//Serial.println();
//delay(200);
//readEnc2();
//Serial.println();
//delay(200);
//deltaPhi(deltaTheta2, deltaTheta1); // finds heading, x and y locations
//Serial.println();
stop();
readGyro();
delay(1000);
readEnc1();
rturn(outsideWheel);
readGyro();
delay(6500);
readEnc1();
stop();
readGyro();
delay(1000);
readEnc1();
strait();
readGyro();
delay(2000);
readEnc1();
stop();
readGyro();
delay(1000);
readEnc1();
lturn(outsideWheel);
readGyro();
delay(6500);
readEnc1();
stop();
readGyro();
delay(2000);
readEnc1();
}