Evo Bot
BT control
Diff: robot.cpp
- Revision:
- 0:603c28b75dc1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.cpp Tue May 20 07:42:25 2014 +0000
@@ -0,0 +1,381 @@
+/* mbed ROBOT Library, for SUTD evobot project, Generation 1
+ * Copyright (c) 2013, SUTD
+ * Author: Mark VanderMeulen
+ *
+ * Dec 18, 2013
+ *
+ * You may want to rewrite this entire library, as this one is only half-finished
+ *
+ * This library allows the user to use each module on the Evobot Generation 1
+ * Functionality:
+ * -Bluetooth connection
+ * -MPU (accelerometer/gyroscope) connection
+ * -Orientation in the XY plane (using Z-axis gyroscope only)
+ * -Remote control functionality (via bluetooth)
+ *
+ * Future functions:
+ * -Switch to turn bluetooth on/off
+ * -Switch to turn MPU on/off (in case of connection errors with the MPU: restart and reconnect
+ * -Calculation of distance travelled (use integration of accelerometer, maybe use Kalman filter)
+ * -Access to the DMP (Digital motion processor) on the MPU to calculate 3D orientation.
+ * -RF mesh network so robots can connect to eachother
+ * -Camera (requires an add-on to the circuit board at the moment)
+ */
+
+
+#include "robot.h"
+#include "math.h"
+//*********************************CONSTRUCTOR*********************************//
+Robot::Robot() : bt(tx_bt,rx_bt),
+ rf24(PTD2, PTD3, PTD1, PTD0, PTD5, PTD4),
+ mpu(PTE0, PTE1),
+ myled(LED),
+ btSwitch(PTE25),
+ currentSensor(CURRENTSENSOR_PIN),
+ irSensorL(irL),
+ irSensorC(irC),
+ irSensorR(irR),
+ voltageSensor(VOLTAGESENSOR_PIN),
+ PWMA(MOT_PWMA_PIN),
+ PWMB(MOT_PWMB_PIN),
+ AIN1(MOT_AIN1_PIN),
+ AIN2(MOT_AIN2_PIN),
+ BIN1(MOT_BIN1_PIN),
+ BIN2(MOT_BIN2_PIN),
+ STBY(MOT_STBY_PIN)
+{
+
+ stop(); //Initialize motors as stopped//
+
+ btSwitch = 1; //turn on bluetooth
+ myled = 0; //turn ON status LED (0 = on, 1 = 0ff)
+ timeNext = 0;
+ //currentAvg = 0; /////////////REMOVE currentAvg LATER. TESTING PURPOSES ONLY////////
+
+ target_angle = 0; //direction we want the robot to be pointed
+ angle_origin = 0; //you can use this to modify the angle of origin
+ origin = 0; //the (x,y) location of the origin (this should be a point, not a double)
+ rz = 0; //The current rotation in the Z-axis
+ dx = 0; //The current displacement in the x-axis (side-side)
+ dy = 0; //The current displacement in the y-axis (forward-back)
+ dz = 0; //The current displacement in the z-axis (up-down)
+
+ AUTO_ORIENT = 1; //robot will automatically orient iteslf using the gyroscope
+ REMOTE_CONTROL = 1; //robot can be controlled over bluetooth
+
+ tt.start(); //start timer
+ mpu.testConnection();
+ wait(1);
+ MPU_OK = 0;
+
+ //initialize MPU
+ if (mpu.testConnection()) {
+ mpu.setBW(MPU6050_BW_10); //default set low pass filter bandwidth to 10HZ
+ mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //default set the gyro range to 500deg/s (robot turning exceeds 250deg/s)
+ mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); //default set accelero range to 2g
+ MPU_OK=1;
+ myled = 0; //turn on LED
+ calibrate();
+ } else if(0) { //this section is current disabled. change to (1) if you want to retry the accelerometer connection.
+ myled = 1; //turn off LED
+ for (int i = 0; i<25; i++) {
+ myled = !myled;
+ wait_ms(50);
+ if (mpu.testConnection()) {
+ i = 25;
+ myled = 0;
+ MPU_OK=1;
+ } else
+ myled = 1;
+ wait_ms(50);
+ }
+ }
+ myled = MPU_OK; //If LED is off, it is ok. If LED is on, there was a MPU error. Board needs to be restarted.
+ //In the future, at a transistor to switch on/off the mpu in case there is an MPU error. Right now it needs a manual restart.
+
+ bt.baud(BaudRate_bt); //Set bluetooth baud rate
+
+ //Initialize RF chip
+//char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
+// int txDataCnt = 0;
+// int rxDataCnt = 0;
+ rf24.powerUp();
+ rf24.setTransferSize( TRANSFER_SIZE );
+
+ rf24.setReceiveMode();
+ rf24.enable();
+
+
+
+}
+
+//*********************************MOTORS*********************************//
+void Robot::motor_control(int Lspeed, int Rspeed)
+{
+ //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
+ if (!Lspeed && !Rspeed) //stop//
+ STBY = 0;
+ else
+ STBY = 1;
+
+ //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
+ if(Lspeed > 100) Lspeed = 100;
+ else if (Lspeed < -100) Lspeed = -100;
+ else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15
+ else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15;
+ if(Rspeed > 100) Rspeed = 100;
+ else if (Rspeed < -100) Rspeed = -100;
+ else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15;
+ else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15;
+
+ if (!Rspeed) { //if right motor is stopped
+ AIN1 = 0;
+ AIN2 = 0;
+ PWMA = 0;
+ } else if (!Lspeed) { //if left motor is stopped
+ BIN1 = 0;
+ BIN2 = 0;
+ PWMB = 0;
+ }
+ //RIGHT MOTOR//
+ if(Rspeed > 0) { //Right motor fwd
+ AIN1 = MOTOR_R_DIRECTION; //set the motor direction
+ AIN2 = !AIN1;
+ } else { //Right motor reverse
+ AIN2 = MOTOR_R_DIRECTION;
+ AIN1 = !AIN2;
+ }
+ PWMA = abs(Rspeed/100.0);
+ //LEFT MOTOR//
+ if(Lspeed >0) {
+ BIN1 = MOTOR_L_DIRECTION;
+ BIN2 = !BIN1;
+ } else {
+ BIN2 = MOTOR_L_DIRECTION;
+ BIN1 = !BIN2;
+ }
+ PWMB = abs(Lspeed/100.0);
+}
+
+void Robot::stop()
+{
+ motor_control(0,0);
+}
+void Robot::set_speed(int Speed)
+{
+ speed = Speed;
+ motor_control(speed,speed);
+}
+
+//************************Setting direction of robot****************************//
+void Robot::set_direction(double angle)
+{
+ //Set the direction the robot should be facing from origin(in radians)
+ target_angle = angle;
+}
+void Robot::set_direction_deg(double angle)
+{
+ //Set the direction the robot should be facing from origin (in degrees)
+ target_angle = angle*M_PI/180;
+}
+void Robot::auto_enable(bool x)
+{
+ AUTO_ORIENT = x;
+}
+
+//************************UPDATE: To be called in main loop***************************//
+//This calculates position and angle
+//Also orients robot in the correct direction
+//if 'print' is 1, print acceleration and gyro data over bluetooth
+void Robot::update()
+{
+ double SP;
+ int rotationSpeed;
+ //int n=50;
+
+ double Kp = 100; //weighting values for PID controller. Only Proportional component is used.
+ double Ki = 0; //Integral component is not used
+ double Kd = 0; //derivative component is not used
+
+ if(MPU_OK) { //only do this if MPU is connected
+ wait(0.1);
+ mpu.getAcceleroRaw(accdata);
+ mpu.getGyroRaw(gyrodata);
+ time = tt.read();
+//bt.printf("s;t_%d;1_%lf;2_%lf;3_%d;4_%d;5_%d;6_%d;7_%d;8_%d;9_%lf;10_%lf;11_%lf;s\n\r",time_robot,getCurrent(),getVoltage(),accdata[0],accdata[1],accdata[2],gyrodata[0],gyrodata[1],gyrodata[2],irReadL(),irReadC(),irReadR());
+ /*if(REMOTE_CONTROL) { //if remote control over bluetooth is enabled
+ m_ctrl=remote_ctrl(); //call the romote control function
+ }*/
+ if(AUTO_ORIENT) { //enable PID control of angle
+ SP = target_angle; //desired angle (radians)
+ rz = rz + ((gyrodata[2]-gyroOffset[2])*(time-timePrev)/gyroCorrect); //rz is the rotation(radians) from start
+ Irz = Irz + (rz-SP)*(time-timePrev); //Irz needs to be reset every so often, or it should be ignored
+ rotationSpeed = (Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect);
+ /*data[0]= getCurrent();
+ data[1]= getVoltage();
+ data[2]= accdata[0];
+ data[3]= accdata[1];
+ data[4]= accdata[2];
+ data[5]= gyrodata[0];
+ data[6]= gyrodata[1];
+ data[7]= gyrodata[2];
+ data[8]= irReadL();
+ data[9]= irReadC();
+ data[10]= irReadC();
+ data[11]= rz;*/
+
+ //TODO: pull "rotationspeed" up to 10 if it is less than 10. This will(should) improve the ability to drive straight
+
+ if(time > timeNext || (speed==0 && rotationSpeed ==0)) { //prevent the motor control from being set too often
+ timeNext = time + MOTOR_INTERVAL/1000.0; //only set the motor speed every 10ms
+ // if(abs(rz-SP)>(10*M_PI/180))
+ // {
+ // motor_control((speed+rotationSpeed)/20, (speed-rotationSpeed)/20); //Set motor speeds
+ //}
+ // else
+ //{
+ motor_control((speed+rotationSpeed), (speed-rotationSpeed)); //Set motor speeds
+ //}
+ }
+ }
+ timePrev = time;
+ }
+}
+
+void Robot::remote_ctrl(char c,int desired_speed,int desired_angle)
+{
+ //This is a private function
+ Irz = 0;
+ switch (c) {
+ case ctrl_forward:
+ speed = -1*desired_speed;
+ break;
+ case ctrl_backward:
+ speed = 1*desired_speed;
+ break;
+ case ctrl_right:
+ target_angle += 90*M_PI/180;
+ break;
+ case ctrl_left:
+ target_angle -= 90*M_PI/180;
+ break;
+ case ctrl_calibrate:
+ calibrate();
+ break;
+ case ctrl_turn_angle_cw:
+ target_angle +=desired_angle*M_PI/180;
+ break;
+ case ctrl_turn_angle_ccw:
+ target_angle -=desired_angle*M_PI/180;
+ break;
+ default:
+ speed = 0;
+ stop();
+ break;
+ }
+}
+//*********************************CALIBRATE*********************************//
+void Robot::calibrate()
+{
+ stop();
+ wait(1.5);
+ double timeNOW = tt.read();
+ int count=0;
+ int i;
+
+ //set the accelerometer and gyro offsets
+ while(tt.read()<timeNOW+1.5) { //calculate gyro offset
+ mpu.getGyroRaw(gyrodata);
+ mpu.getAcceleroRaw(accdata);
+ for(i=0; i<3; i++) {
+ gyroOffset[i] += gyrodata[i];
+ accOffset[i] += accdata[i];
+ }
+ count++;
+ }
+ for(i=0; i<3; i++) {
+ gyroOffset[i] = gyroOffset[i]/count; //rxOffset
+ accOffset[i] = accOffset[i]/count;
+ }
+ accOffset[2] = 0; //we don't want to remove GRAVITY from the z-axis accelerometer.
+}
+
+//*********************************ROBOT-SENSORS*********************************//
+
+double Robot::getCurrent()
+{
+
+ double Vsensor = currentSensor.read();
+ Vsensor = 1000*((.8998-Vsensor)/.6411);
+ return Vsensor;
+}
+double Robot::getVoltage()
+{
+ float voltage = 3.3*(voltageSensor.read()); // convert analog value to voltage at node
+ voltage *= 1.5; // inverse of voltage divider
+ return voltage;
+}
+void Robot::getIMU(float *adata, float *gdata)
+{
+ mpu.getAcceleroRaw(accdata);
+ mpu.getGyroRaw(gyrodata);
+ /*imuFilter.updateFilter(gyrodata[0],gyrodata[1],gyrodata[2],accdata[0],accdata[1],accdata[2]);
+ imuFilter.computeEuler();
+ gdata[0]=(float)imuFilter.getRoll();
+ gdata[1]=(float)imuFilter.getPitch();
+ gdata[2]= (float)imuFilter.getYaw();*/
+ // adata[0] = (float) (accdata[0]);
+ //adata[1] = (float) (accdata[1]);
+ //adata[2] = (float) (accdata[2]);
+ adata[0] = ((float) (accdata[0]-accOffset[0]))/32655*9.81*2;
+ adata[1]= ((float) (accdata[1]-accOffset[1]))/32655*9.81*2;
+ adata[2]= ((float) (accdata[2]-accOffset[2]))/32655*9.81*2;
+ gdata[0]=(((float)(gyrodata[0]-gyroOffset[0]))/((float)gyroCorrect))*180/(M_PI);
+ gdata[1]=(((float)(gyrodata[1]-gyroOffset[1]))/((float)gyroCorrect))*180/(M_PI);
+ gdata[2]=(((float)(gyrodata[2]-gyroOffset[2]))/((float)gyroCorrect))*180/(M_PI);
+}
+
+double Robot::irReadL()
+{
+ int C = 14;
+ double D = -1.1;
+ double r = irSensorL.read();
+ double reading=(pow(r,D)* C )+ 4;
+ return reading;
+
+}
+
+double Robot::irReadC()
+{
+ int C = 14;
+ double D = -1.1;
+ double r = irSensorC.read();
+ double reading=(pow(r,D)* C )+ 4;
+ return reading;
+}
+
+double Robot::irReadR()
+{
+ int C = 14;
+ double D = -1.1;
+ double r = irSensorR.read();
+ double reading=(pow(r,D)* C )+ 4;
+ return reading;
+}
+double Robot::return_rotation()
+{
+ return rz;
+}
+int Robot::isMPU()
+{
+ return MPU_OK;
+}
+
+//******************RF24 CHIP FUNCTIONS****************************//
+void Robot::rf24_power(int x)
+{
+ if(x) //power up
+ rf24.powerUp();
+ else //power down
+ rf24.powerDown();
+}
\ No newline at end of file