For the SUTD research project. This is a sample code to access the MPU6050
Dependencies: MPU6050 Terminal mbed
Diff: main.cpp
- Revision:
- 0:59f6f94baeb1
diff -r 000000000000 -r 59f6f94baeb1 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Dec 01 06:22:13 2013 +0000 @@ -0,0 +1,392 @@ +/** 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); + +}