Mark Vandermeulen
For the SUTD research project. This is a sample code to access the MPU6050
Dependencies: MPU6050 Terminal mbed
Diff: main.cpp
- Revision:
- 0:59f6f94baeb1
--- /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);
+
+}