For the SUTD research project. This is a sample code to access the MPU6050
Dependencies: MPU6050 Terminal mbed
main.cpp
- Committer:
- majik
- Date:
- 2013-12-01
- Revision:
- 0:59f6f94baeb1
File content as of revision 0:59f6f94baeb1:
/** This program constantly sends the accelerometer data over the bluetooth to the computer. * The robot will drive FWD and REVERSE at maximum speed when you press W and S. //Turn 90 degrees at a time// * TODO: implement 90deg rotation * TODO: Get rid of flags. Turn everything into a class * TODO: Use a PID controller for turning instead of whatever mess you used today */ #include "mbed.h" #include "MPU6050.h" #include "Terminal.h" #include "string" #include "main.h" //const int startMotor = 2; //time (in seconds) when motor starts //const int stopMotor = 4; //time (in seconds) when motor stops Terminal pc(PTA2,PTA1); //bluetooth DigitalOut myled(PTE3); //LED MPU6050 mpu(PTE0, PTE1); //Accelerometer (needs the MPU6050 library) //things for the motor controller PwmOut PWMA(PTA4); PwmOut PWMB(PTA5); DigitalOut AIN1(PTA14); DigitalOut AIN2(PTA13); DigitalOut BIN1(PTA16); DigitalOut BIN2(PTA17); DigitalOut STBY(PTA15); //motor functions void motor_control(bool motor, int speed, bool dir); //speed should be between 1 and 100 void stop(); void straight(int speed); //drive robot straight. Speed between -100 and 100. Accelerometer works best at 100 void turn_left(int speedA, int speedB); void turn_right(int speedA, int speedB); double integrate(data *prev, int acc[3], int time); //integrate. this is not implemented yet void remote_control(char c, int *turn); //Send a cmmand to this function, it will drive the robot int accFlag = 0; //flag to continue acceleration or deceleration (1 for acc, 0 for none, -1 for dec) int turnFlag = 0; //flag for direction of turn. If 0, no turn, if 1 turn right, -1 turn left int accLimit = 16384.0/9.81* 1; //acceleration limit = 2 m/s2 int maxRot = 0; int main() { double t2 = 0; double tnow; double SP; int Kp = 75; //proportional gain (tuning) int Ki = 0; //integral gain (tuning) double Kd = 0; //derivative gain (tuning) *Not used = 0* double Irz = 0; double rotationSpeed=0; double rz=0; //rotation in the z axis double tprev=0;//time of previous iteration double pi = 3.14159265359; int i=0; int turnTarget = 0; //angle for the robot to drive at (in degrees) // data *state; //this will be passed into the integrate fucntion for recursiveness //send data to the integrate function, delayed by 10 samples // char c; // int flag = 0; int accdata[3]; int avAcc[3]; int gyrodata[3]; Timer t; pc.baud(115200); //Make sure this baud rate matches the baud rate of the bluetooth (set baud rate with "BluetoothAT") wait(0.1); char test = mpu.testConnection(); pc.baud(115200); //this can only be changed after the BLUETOOTH baud rate has been changed if (mpu.testConnection()){ //pc.printf("MPU connection succeeded\n\r"); myled = 0; } else{ pc.printf("MPU connection failed\n\r"); //Todo: If connection fails, retry a couple times. Try resetting MPU (this would need another wire?) myled = 1; } wait(3); t.reset(); // mpu.setBW(MPU6050_BW_10); //set bandwidth of low pass filter for Accerlerometer AND Gyro. // mpu.setBW(MPU6050_BW_42); // mpu.setBW(MPU6050_BW_98); mpu.setBW(MPU6050_BW_20); // mpu.setGyroRange(MPU6050_GYRO_RANGE_250); //To convert gyro output to RAD/S, divide by 7505.7 mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //to convert gyro output to RAD/S, divide by 3752.9 //double gyroCorrect = 3752.9; //////change this number!!!!Decrease to decrease rotation double gyroCorrect = 3720; //prev3754.82prev3752.9 Use this number to convert gyro reading to rad/s int count = 0; double rzOff = 0; while(t.read()<1) //calculate gyro offset { mpu.getGyroRaw(gyrodata); rzOff += gyrodata[2]; count++; } rzOff = rzOff/count; //rzOffset while(1) { //speed = 100; mpu.getAcceleroRaw(accdata); mpu.getGyroRaw(gyrodata); //ROTATION//------------------------------------// //MV = 0; //input (speed setting) SP = turnTarget*pi/180; //desired angle (radians) tnow = t.read(); rz = rz + ((gyrodata[2]-rzOff)*(tnow-tprev)/gyroCorrect); //rz is the rotation(radians) from start Irz = Irz + (rz-SP)*(tnow-tprev); //Irz needs to be reset every so often, or it should be ignored tprev = tnow; rotationSpeed = (int(Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect)); pc.printf("\n\r%f\t",rotationSpeed); if(maxRot < gyrodata[2]) maxRot = gyrodata[2]; if(abs(rotationSpeed)<1)//(abs(rz-SP)<(0.0175*gyroCorrect)) { rotationSpeed = 0; Irz = 0; } else if(abs(rotationSpeed)<10) { if (rotationSpeed>0) rotationSpeed = 10; else rotationSpeed = -10; } if(tnow>t2) { turn_right(-rotationSpeed,rotationSpeed); t2 = tnow+0.01; } //END ROTATION//---------------------------------// if (accFlag > 0) accFlag --; else if(accFlag < 0) accFlag++; // avAcc[0] = 0; // avAcc[1] = 0; // avAcc[2] = 0; // for(i = 0; i<4; i++) // { // // mpu.getAcceleroRaw(accdata); // mpu.getGyroRaw(gyrodata); // // avAcc[0] += accdata[0]; // avAcc[1] += accdata[1]; // avAcc[2] += accdata[2]; // } pc.printf("%f\t%d\t%d\t%d\t", tnow,accdata[0], accdata[1], accdata[2]); // pc.printf("%d\t%d\t%d\t%d\n\r", gyrodata[0], gyrodata[1], gyrodata[2],STBY.read()); // pc.printf("%f\n\r",tnow); //Old Code for moving the robot// /* if (t.read() > startMotor && flag ==0){ myled = 0; straight(100); flag = 1; } if (t.read() > stopMotor){ stop(); myled = 1; STBY=0; flag = 2; } if (t.read() > 20) break; */ //New code for moving robot// if(pc.readable() || accFlag){ if(accFlag) c = 'i'; else c = pc.getc(); remote_control(c,&turnTarget); if (c=='t'){ //reset timer if t is pressed wait(2); t.reset(); turnTarget = 0; } } } } void motor_control(bool motor, int speed, bool dir) //motor: 0=right, 1=left; speed: 0-100; dir 0=fwd, 1=bkwd { if (speed == 0) //stop// { STBY = 0; if (motor ==0) { AIN1 = 0; AIN2 = 0; PWMA = 0; } else { BIN1 = 0; BIN2 = 0; PWMB = 0; } } else STBY = 1; if (!dir) //forward// { if(motor == 0) //right motor// { AIN1 = 1; AIN2 = 0; PWMA = abs(speed/100.0); } else //left motor// { BIN1 = 0; //REVERSE THESE WHEN FIX WIRING//////*****////// BIN2 = 1; PWMB = abs(speed/100.0); } } else { if(motor == 0) //right motor// { AIN1 = 0; AIN2 = 1; PWMA = abs(speed/100.0); } else //left motor// { BIN1 = 1; //REVERSE THESE WHEN FIXED WIRING!!////*****//////// BIN2 = 0; PWMB = abs(speed/100.0); } } } void stop() { motor_control(0,0,0); //motorA, speed=0, direction = fwd motor_control(1,0,0); STBY=0; } void straight(int speed) //if speed is +ve, go straight. -ve reverse { if (speed == 0) stop(); else if (speed > 100) speed = 100; else if (speed < -100) speed = -100; else if (speed>0) //fwd// { motor_control(0,speed,0); motor_control(1,speed,0); } else { motor_control(0,speed,1); motor_control(1,speed,1); } } double integrate(int *prev, int acc[3], int time) { /*Returns the distance travelled*/ return 0; } /*Makes robot go straight, stop, and turn*/ void remote_control(char c, int *turn) { int speed; int y; switch (c) { case 'w': //forward - increment speed // turn=0; speed = 100; straight(speed); break; case 's': //backwards // turn=0; speed = -100; straight(speed); break; case 'd': //right /*if(BIN1.read()==0 && AIN1.read()==0) //if previously turning right, go straight stop(); //{ // turn = 0; // turnRate = 0; // straight(speed); //} else { //turn_left(50,-50); }*/ *turn+=90; //rotate right 90 degrees break; case 'a': //left /*if(BIN1.read() && AIN1.read()) //if previously turning left, go straight; stop(); else turn_right(-50,50); */ *turn-=90; //rotate left 90 degrees break; case 'i': //constant acceleration //if(BIN1.read() || (AIN!.read() == 0)) //if previously reverse, stop // stop(); //else //go in reverse, at a constant acceleration speed = (PWMA.read() + PWMB.read())/2*100; y = mpu.getAcceleroRawY(); if(speed<100 || BIN1.read() || (AIN1.read()==0) && accFlag < 2) { accFlag = 5; if(BIN1.read()) stop(); if(abs(y)<accLimit) { speed = speed + 1; straight(speed); } } else accFlag = 0; break; default: //stop speed = 0; stop(); } } void turn_left(int speedA, int speedB) { if (speedA < 0) //R reverse// { motor_control(0,speedA,1); //Right reverse motor_control(1,speedB,1); //Left reverse } else if(speedB <= 0) { motor_control(1,speedB,1); //Left reverse// motor_control(0,speedA,0); //Right fwd } else { motor_control(0,speedA,0); //Right fwd motor_control(1,speedB,0); //Left fwd } } void turn_right(int speedA, int speedB) { if (speedA > 0) //R fwd// { motor_control(0,speedA,0); //Right fwd } else { motor_control(0,speedA,1); //Right reverrse } if(speedB<0) //L rev { motor_control(1,speedB,1); //Left reverse } else motor_control(1,speedB,0); }