void calibrate_accel(){
xoff = accelgyro.getXAccelOffset();
yoff = accelgyro.getYAccelOffset();
zoff = accelgyro.getZAccelOffset();
byte i=0;
while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
accelgyro.getAcceleration(&ax, &ay, &az);
if (count == SAMPLE_COUNT){
xoff += int(axm/-6);
yoff += int(aym/-6);
zoff += int((azm+16384)/-6);
accelgyro.setXAccelOffset(xoff);
accelgyro.setYAccelOffset(yoff);
accelgyro.setZAccelOffset(zoff);
#ifdef CAL_DEBUG
Serial.print(axm); Serial.print(" ");
Serial.print(aym); Serial.print(" ");
Serial.println(azm);
Serial.print(xoff); Serial.print(" ");
Serial.print(yoff); Serial.print(" ");
Serial.println(zoff);
Serial.println("*********************");
#endif
count = 0;
i++; //iteration++
#ifdef CAL_DEBUG
Serial.print(".");
#endif
}
else{
axm = (axm*count + ax)/(count+1.0);
aym = (aym*count + ay)/(count+1.0);
azm = (azm*count + az)/(count+1.0);
count++;
}
}
#ifdef CAL_DEBUG
Serial.println(" Done.");
#endif
}
int main() {
// MPU6050 object "MPU6050.h"
MPU6050 accelgyro;
// Kalman filter Object "Kalman.h"
Kalman kalman;
// Xbee serial baudrate
xBee.baud(38400);
pc.baud(9600);
// Initialize timers
Timer timer, code, encTimer;
// Start timers
timer.start();
code.start();
encTimer.start();
// Reset timer values
timer.reset();
code.reset();
encTimer.reset();
//Encoder 1 interrupts
enc1a.rise(&incrementEnc1a);
enc1b.rise(&incrementEnc1b);
enc1a.fall(&setEnc1aFall);
enc1b.fall(&setEnc1bFall);
enc2a.rise(&incrementEnc2a);
enc2b.rise(&incrementEnc2b);
enc2a.fall(&setEnc2aFall);
enc2b.fall(&setEnc2bFall);
// Debug leds
myled1 = 0;
myled2 = 0;
myled3 = 0;
// Encoder 1 initializations
newTime =0; oldTime = 0; lastEncTime = 0;
enc1Speed = 0; enc2Speed = 0;
// MPU6050 initializations
accelgyro.initialize();
accelgyro.setFullScaleGyroRange(0);
accelgyro.setFullScaleAccelRange(0);
// Initialize MotorShield object
shield.init();
float measuredTheta , measuredRate, newTheta,calcRate;
float position= 0, velocity = 0, lastPosition = 0, lastVelocity =0;
float angleOffset = 0, positionOffset = 0,lastAngleoffset = 0;
// Position control constants, if necessary
float zoneA = 16000.0f, zoneB = 8000.0f, zoneC = 2000.0f;
float multiplier = 1.0f;
float zoneAscale = 600*2*multiplier,
zoneBscale = 800*2*multiplier,
zoneCscale = 1000*2*multiplier,
zoneDscale = 1500*2*multiplier,
velocityScale = 60*2*multiplier;
// Serial communication buffer
char buffer[40];
int i, strSize;
// Control parameters
float k0,k1,k2,k3,tref;
float x, dotx, deltaX, deltaT, lastX = 0;
// Helper variables
float waittime , u, diff, dt;
float error = 0,lastError = 0;
int counter = 0;
float pTerm = 0,dTerm = 0,iTerm = 0;
while(1) {
///////////////////////////////////////////
// Read serial data and update constants //
///////////////////////////////////////////
i = 0;
char a;
while(pc.readable()){
a = pc.getc();
buffer[i++] = a;
printf("%c\n", a );
myled1 = 1;
wait(0.001);
}
strSize = i;
string receive(buffer,strSize); // Changes from char to string
if(strSize > 0){
printf("%s\n", receive);
assignGainsFromString(&k[0], &k[1], &k[2], &k[3], &k[4],receive);
}
// Below is an attempt to control the robot position,
// by dividing it into "zones" and changing the angle accordingly.
//
/////////////////////////////
// Generate encoder offset //
/////////////////////////////
// position = (float)enc2total;
// positionOffset = position;
// //if((encTimer.read_us() - lastEncTime) > 0.0f ) { // every 100 ms
// float sign = 0;
// if(positionOffset > 0) sign= 1;
// else sign = -1;
// if(abs(positionOffset) > zoneA) angleOffset += 1.0f/zoneAscale*positionOffset;
// else if(abs(positionOffset) > zoneB) angleOffset += 1.0f/zoneBscale*positionOffset;
// else if(abs(positionOffset) > zoneC) angleOffset += 1.0f/zoneCscale*positionOffset;
// else angleOffset += positionOffset/zoneDscale;
// printf("angleOffset: %f, positionoffset: %f\n", angleOffset, positionOffset );
// // Estimate velocity
// //
// velocity = (position - lastPosition);
// lastPosition = position;
// angleOffset += velocity/velocityScale;
// angleOffset = constrain(angleOffset,-10, 10);
// lastAngleoffset = angleOffset;
// //}
// angleOffset = constrain(angleOffset,lastAngleoffset - 1, lastAngleoffset + 1);
timer.reset();
///////////////////////////
// Get gyro/accel values //
///////////////////////////
accelgyro.getAcceleration(&ax,&ay,&az);
measuredRate = accelgyro.getRotationX()*1.0/131.0f; // Units depend on config. Right now 250o/s
measuredTheta = -atan(1.0*ay/az);
measuredTheta = measuredTheta*180/PI; // measured in degrees
///////////////////
// Kalman Filter //
///////////////////
dt = (double)(code.read_us() - oldTime)/1000000.0f;
newTheta = kalman.getAngle(measuredTheta,
-measuredRate, dt);
//DEBUG: printf("%g \n", (double)(code.read_us() - oldTime)/1000000.0f);
oldTime = code.read_us();
//////////////////
// Control loop //
//////////////////
// Set control variable to zero
u = 0;
// Extract constants from k vector, which has the serial readings.
float kp = k[0];
float ki = k[1];
float kd = k[2];
tref = k[3] - angleOffset;
waittime = k[4];
if(newTheta >= 50 || newTheta <= -50){
u = 0;
}else{
// Define error term
error = newTheta - tref;
// Proportional term
pTerm = kp*error;
//Integral term
iTerm += ki*error*dt*100.0f;
// Prevent integration windup:
if(iTerm >= 100) iTerm = 100;
else if (iTerm <= -100) iTerm = -100;
u = -(iTerm + pTerm);
// Calculated derivative based on smoothened angle.
// There are two other sources for the angle here: kalman.getRate(); and measuredRate.
calcRate = -(error-lastError)/dt;
// Derivative term
if(kd != 0)
dTerm = kd*calcRate/100.0f;
lastError = error;
u += dTerm;
// Correct for dead zone --- Did not have successful results but I'll leave it here.
// int deadzone = 20;
// if(u < -deadzone) u = u + deadzone;
// else if(u > deadzone) u = u - deadzone;
// else u = 0;
// // Include saturation
u = constrain(u,-400,400);
// Flash LED to indicate loop
if(counter % 50 == 0){
myled3 = !myled3;
counter = 0;
}
counter++;
}
lastError = error; // update angle
if(counter % 50 == 0){
myled3 = !myled3;
// xBee.printf("%f,%f\n", newTheta,u);
counter = 0;
}
// Set motor speed
shield.setM1Speed(-(int)u);
shield.setM2Speed((int)u);
// DEBUG over serial port. Use with MATLAB program "serialPlot.m" to plot these results.
printf("%f,%f,%f\n", measuredTheta, newTheta , u);
// Hold the timer until desired sampling time is reached.
while(timer.read_us() < waittime*1000);
// Turn off serial transmission LED
myled1 = 0;
}
}
// PUBLIC METHOD TO PROVIDE DATA ACQUIRED FROM MPU TO DEPENDENT SYSTEMS
uint8_t mpuAcquire(int16_t *accelOffsetX, int16_t *accelOffsetY,int16_t *accelOffsetZ, int16_t *currAccelX, int16_t *currAccelY, int16_t *currAccelZ, int16_t *currYaw, int16_t *currPitch, int16_t *currRoll){ /*Changed to int16*/
int16_t rot[3]; //Equivalent to uint8_t yaw; uint8_t pitch; uint8_t roll;
int16_t accel[3];//Equivalent to uint8_t accelX; uint8_t accelY; uint8_t accelZ;
//Error flag
uint8_t errorCode;
//DEFAULT SEND 0's
for(uint8_t j=0; j<4;j++){
accel[j]=0;
}
for(uint8_t k=0; k<4;k++){
rot[k]=0;
}
//Monitor MPU
errorCode=mpuMonitor(currAccelX,currAccelY,currAccelZ);
//IF PROBLEM, SEND PREVIOUS VALUES
if(errorCode!=0){
accel[0]=*currAccelX;
accel[1]=*currAccelY;
accel[2]=*currAccelZ;
rot[0]=*currYaw;
rot[1]=*currPitch;
rot[2]=*currRoll;
}
//IF GOT VALUES, SUPPLY NEW VALUES
else if(errorCode==0){
// display raw acceleration values
// Chosen getAcceleration method because only one showing approximately independent dimensions
// ALL others having problems; linearAccelInWorld in particular only giving zeroes, and linearAccel giving all ~same
#ifdef MPU_RAW_ACCEL
mpu.getAcceleration(accel,accel+1,accel+2);
accel[0]=(accel[0] - *accelOffsetX); // Components with offset applied
accel[1]=(accel[1] - *accelOffsetY); // No scaling factor 2048, to maximize resolution in integer rep.
accel[2]=(accel[2] - *accelOffsetZ);
#endif
// TO DO: WILL DETERMINE AXIS OF MAX DIFFERENCE BETWEEN accel AND currAccel VALUES...
// AND DIVIDE OTHER AXES' VALUES BY DERIVATIVE (USING TIMER COUNT)
// -OR- TAKE TIME AVERAGE OF BUFFERED VALUES AND DIVIDE OTHERS BY AXIS OF MAX RATE OF CHANGE
// -OR- MAX/MIN HOLD VALUES ON AXIS AND FIND RATE OF CHANGE OF PEAKS. DIVIDE OTHER AXES BY MAX RATE OF CHANGE
// display raw gyroscope values
//Took raw values because all others not independent (and dmpGetGyro in particular not sensitive)
#ifdef MPU_RAW_GYRO
mpu.getRotation(rot,rot+1,rot+2);
/* rot[0]=(rot[0]/16.4); // No scaling of rotation, so as maximize resolution with integer
rot[1]=(rot[1]/16.4);
rot[2]=(rot[2]/16.4); */
//Return absolute angle values, if desired
/* #ifdef ABSOLUTE_ANGLE
//Apply complementary filter to get
Complementary_Filter(accel[0],accel[1],accel[2],rot[0],rot[1],rot[2]);
#endif */
#endif
}
//Print out and update "previous" values to current ones
#ifdef MPU_RAW_ACCEL
/* FOR DEBUG, SERIAL PRINT
#ifdef accelAsComponents
Serial.print(accel[0] + ", " + accel[1] + ", " + accel[2]));
#endif
#ifdef accelAsMag
Serial.print(sqrt((abs(accel[0]))^2+(abs(accel[1]))^2+(abs(accel[2]))^2));
#endif
*/
*currAccelX=accel[0];
*currAccelY=accel[1];
*currAccelZ=accel[2];
#endif
#ifdef MPU_RAW_GYRO
*currYaw=rot[0];
*currPitch=rot[1];
*currRoll=rot[2];
#endif
/* #ifdef GET_TAPS
if(&(devAddr+TAPXYZ)){ //If tap received register goes high, send 1 tap indication
Serial.print(", 1");
}
#ifdef MANUAL_TAP
else if(gotTaps(rot[0],rot[1],rot[2],sFIFO_Y,sFIFO_P,sFIFO_R)){ //Same from gotTap() function, if returns true
Serial.print(", 1");
}
#endif
#endif */
//Return error code, to indicate type of problem if any
return errorCode;
}
void system_init()
{
DEBUG_PRINTF(V_MESSAGE, "Initializing WirePi.\n");
wiringPiSetup() ;
pinMode(LED, OUTPUT);
LED_ON; // Turn red led ON on initialization.
DEBUG_PRINTF(V_MESSAGE, "Initializing timers.\n");
init_photo_timer();
if((operation_type == OPERATION_A)
|| (operation_type == OPERATION_B))
{
init_accelgyro_timer();
}
init_operation_timer();
init_led_timer();
DEBUG_PRINTF(V_MESSAGE, "Initializing accelgyro.\n");
accelgyro.initialize();
if(operation_type == OPERATION_A) // PLAN A
{
accelgyro_handler = &accelgyro_handler_planA;
accelgyro.getAcceleration(&x, &y,&z);
accel_low.x = accel_low.x * gravity_alpha + x * (1 - gravity_alpha);
accel_low.y = accel_low.y * gravity_alpha + y * (1 - gravity_alpha);
accel_low.z = accel_low.z * gravity_alpha + z * (1 - gravity_alpha);
last_angle = accel_low.getAngleTo(gravity);
max_angle = last_angle;
start_accelgyro_timer(0, ACCELGYRO_TICK);
}
else if(operation_type == OPERATION_B) // PLAN B
{
accelgyro_handler = &accelgyro_handler_planB;
start_accelgyro_timer(0, ACCELGYRO_TICK);
}
DEBUG_PRINTF(V_MESSAGE, "Initializing TCP socket.\n");
socket_tcp = TCP_Socket(host_ip, socket_port);
DEBUG_PRINTF(V_MESSAGE, "Starting Timers.\n");
start_led_timer(TIMER_LED_TICK, 0);
DEBUG_PRINTF(V_MESSAGE, "Initialization sleep!\n");
sleep(INITIAL_DELAY);
DEBUG_PRINTF(V_MESSAGE, "System initialized!\n");
if((operation_test == TEST_NO_TCP) || (operation_test == FULL_TEST))
{
DEBUG_PRINTF(V_MESSAGE, "System test!\n");
if(system_test() == false)
{
signalize(ERROR);
DEBUG_PRINTF(V_ERROR, "Failed on System Test!\n");
while(1);
}
else
{
DEBUG_PRINTF(V_MESSAGE, "Passed on system test!\n");
}
}
return;
}
uint8_t mpuMonitor(int16_t *currAccelX,int16_t *currAccelY,int16_t *currAccelZ){
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint16_t fifoCount; // count of all bytes currently in FIFO
/*MUST DECIDE WHETHER TO USE*/
/*ERROR CODES:
1: DMP not ready
2: No interrupt received
3: FIFO OFLOW
4: Other (unknown)
*/
uint8_t monitorErrorCode=0;
//PROGRAMMING FAILURE CHECK
if (!dmpReady){
monitorErrorCode=1;
return monitorErrorCode;
}
//NO-INTERRUPT CHECK
// If fails, must wait for MPU interrupt or extra packet(s) to become available
// Also catches if interrupt line disconnected, or other hardware issues (e.g. power loss)
if(!mpuInterrupt && (fifoCount < packetSize)){
monitorErrorCode=2;
return monitorErrorCode;
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// FIFO OVERFLOW CHECK
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) { // mpu FIFO OFLOW flag is raised or fifoCount has max of 1024 (max # of bytes in buffer)
monitorErrorCode=3;
// reset so we can continue cleanly
mpu.resetFIFO();
return monitorErrorCode;
}
// GOT MPU DATA READY INTERRUPT WITH SUFFICIENT SIZE!
else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
//mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
//mpuMONITOR gets values of acceleration, too...
//to provide for offset calculation in calibration (redundancy - to improve)
mpu.getAcceleration(currAccelX,currAccelY,currAccelZ);
return monitorErrorCode;
}
//Unknown error
monitorErrorCode=4;
return monitorErrorCode;
}
