Revision 0:603c28b75dc1, committed 2014-05-20

Comitter:

Throwbot

Date:

Tue May 20 07:42:25 2014 +0000

Commit message:

BT

Changed in this revision

diff -r 000000000000 -r 603c28b75dc1 robot.cpp
--- /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

diff -r 000000000000 -r 603c28b75dc1 robot.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/robot.h	Tue May 20 07:42:25 2014 +0000
@@ -0,0 +1,177 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include "mbed.h"
+#include "MPU6050.h"
+#include "nRF24L01P.h"
+
+#define BaudRate_bt    9600        //Baud rate of 9600
+#define tx_bt       PTA2        //Bluetooth connection pins
+#define rx_bt       PTA1        //Bluetooth connection pins
+#define tx_mpu      PTE0    //MPU connection pins
+#define rx_mpu      PTE1    //MPU connection pins
+#define mpu_bandwidth MPU6050_BW_20 //set the MPU low pass filter bandwidth to 20hz
+
+#define LED PTE3        //status LED pin
+#define CURRENTSENSOR_PIN PTC2
+#define VOLTAGESENSOR_PIN PTB0
+
+#define CURRENT_R1 180 //160.0     //values of the current sensor opamp resistors
+#define CURRENT_R2 10
+#define CURRENT_R3 80
+#define CURRENT_R4 84.7
+#define VREF3_3 3.3        //digital logic voltage
+#define VREF5 5.0       //5v voltage        //NOTE: 5v voltage is consistent when using new batts, but not using old blue batts
+
+#define irL PTB1
+#define irC PTB3
+#define irR PTB2
+
+#define MOT_PWMA_PIN   PTA4    //Motor control pins    
+#define MOT_PWMB_PIN   PTA5
+#define MOT_STBY_PIN   PTA15
+#define MOT_AIN1_PIN   PTA14
+#define MOT_AIN2_PIN   PTA13
+#define MOT_BIN1_PIN    PTA16
+#define MOT_BIN2_PIN   PTA17
+
+#define M_PI 3.14159265359  //PI
+#define gyroCorrect 3720    //divide raw gyro data by this to get result in RAD/SECOND (if gyroRange is 500 rad/s)
+
+//Correct direction of motors. If number = 1, forward. If number = 0, backwards (for if motors are wired backwards)
+#define MOTOR_R_DIRECTION   1
+#define MOTOR_L_DIRECTION   1
+
+#define MOTOR_INTERVAL 20     //defines the interval (in milliseconds) between when motor can be set
+//NOTE: Can't make this less than 20ms, because that is the PWM frequency
+
+//Key bindings for remote control robot - for the future try to use arrow keys instead of 'asdw'
+#define ctrl_forward    'i'         //forward
+#define ctrl_backward   'k'         //back
+#define ctrl_left       'j'         //turn left
+#define ctrl_right      'l'         //turn right
+#define ctrl_calibrate  'c'         //re-calibrate the accelerometer and gyro
+#define ctrl_turn_angle_cw    'o'         // turn angle
+#define ctrl_turn_angle_ccw    'p'
+// The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
+//  "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
+//  only handles 4 byte transfers in the ATMega code.
+#define TRANSFER_SIZE   4
+
+class Robot
+{
+public:
+    /**
+    *   Constructor - does nothing atm
+    */
+    Robot();
+
+    /**
+    *   MOTOR CONTROLS
+    */
+    void motor_control(int Lspeed, int Rspeed);  //Input speed for motors. Integer between 0 and 100
+    void stop();                           //stop motors
+    void set_direction(double angle);            //set angle for the robot to face (from origin)
+    void set_direction_deg(double angle);
+    void set_speed(int Speed );      //set speed for robot to travel at
+    void auto_enable(bool x);
+    /**
+    *   MPU CONTROLS
+    */
+
+    /**
+    *   UPDATE (this must be called repeatedly so the robot will sample accelerometer to find position)
+    */
+    void update(); //print variable decides which values to print
+    void remote_ctrl(char c,int desired_speed, int desired_angle); 
+    //print = 0: nothing
+    //print = 1: Accelerometer and gyro
+    //print = 2: Current and voltage
+
+    //  calibrate the gyro and accelerometer  //
+    void calibrate();
+
+    /**
+    *   Status: find the distance, orientation, battery values, etc of the robot
+    */
+    //void distanceTravelled(double x[3])
+    //void orientation(something quaternion? on xy plane?)
+    double getCurrent();   //Get the current drawn by the robot
+    double getCurrent(int n); //get the current, averaged over n samples
+    double getVoltage();    //get the battery voltage (ask connor for completed function)
+    void getIMU(float *adata, float *gdata);
+    double irReadL();
+    double irReadC();
+    double irReadR();
+    double return_rotation();
+    int isMPU();
+    
+
+    //Wireless connections
+    Serial bt;  //bluetooth connection
+    nRF24L01P rf24;  //RF network connection
+
+    //RF24 network functions//
+    void rf24_power(int status);   //power on or off the RF network
+    char rf24_read();
+    int rf24_write(char letter);   //write a letter to RF
+    int rf24_write(char* buffer, int length);   //write
+    int acc[3];
+    int gyr[3];
+
+    
+      
+    //-------------------PRIVATE VARIABLES AND FUNCTIONS-----------------------------------------------//
+private:
+
+        //commands for remote control robot
+
+    MPU6050 mpu;   //MPU connection
+    DigitalOut myled;   //(PTE3) Processor LED (1 = off, 0 = on)
+    DigitalOut btSwitch;
+    AnalogIn currentSensor;
+    AnalogIn irSensorL;
+    AnalogIn irSensorC;
+    AnalogIn irSensorR;
+    AnalogIn voltageSensor;
+
+    double dx;  //distance travelled in x direction
+    double dy;  //distance travelled in y direction
+    double dz;  //distance travelled in z direction (zero?)
+    double origin;    //location of robot origin (or can be set if robot starting location is known.
+            double target_angle;    //direction that we want the robot to face (radians)
+
+    int accdata[3];     //data from accelerometer (raw)
+    int gyrodata[3];    //data from gyro (raw)
+    //double gyroCorrect; //= 3720;  //divide by this to get gyro data in RAD/SECOND. This is above as a #DEFINE
+    int gyroOffset[3];  //Correction value for each gyroscope to zero the values.
+    int accOffset[3];  //correction value for each accelerometer
+int speed;  //set the speed of robot
+    /**Angle is always measured in clockwise direction, looking down from the top**/
+    /*Angle is measured in RADIANS*/
+    double rz;          //Direction robot is facing
+    double Irz;         //integral of the rotation offset from target. (Optionally) Used for PID control of direction
+    double angle_origin;      //Angle of origin (can be changed later, or set if robot starts at known angle)
+    bool AUTO_ORIENT; //if this flag is 1, the robot automatically orients itself to selected direction
+    bool REMOTE_CONTROL;    //if this flag is 1, the robot will be controlled over bluetooth
+
+    ///////////   Motor control variables   ///////////
+    PwmOut PWMA;//(MOT_PWMA_PIN);
+    PwmOut PWMB;//(MOT_PWMB_PIN);
+    DigitalOut AIN1;//(MOT_AIN1_PIN);
+    DigitalOut AIN2;//(MOT_AIN2_PIN);
+    DigitalOut BIN1;//(MOT_BIN1_PIN);
+    DigitalOut BIN2;//(MOT_BIN2_PIN);
+    DigitalOut STBY;//(MOT_STBY_PIN);
+    double timeNext;    //next time that the motor is allowed to be updated
+
+    bool MPU_OK;
+
+    Timer tt;    //timer
+    
+    double time;    //time of current iteration
+    double timePrev;    //time of previous iteration
+    
+    
+};
+#endif
\ No newline at end of file