Evo Bot / Robot

BT control

robot.h@0:603c28b75dc1, 2014-05-20 (annotated)

Committer:
Throwbot
Date:
Tue May 20 07:42:25 2014 +0000
Revision:
0:603c28b75dc1
BT

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 0:603c28b75dc1 1 #ifndef ROBOT_H
Throwbot 0:603c28b75dc1 2 #define ROBOT_H
Throwbot 0:603c28b75dc1 3
Throwbot 0:603c28b75dc1 4 #include "mbed.h"
Throwbot 0:603c28b75dc1 5 #include "MPU6050.h"
Throwbot 0:603c28b75dc1 6 #include "nRF24L01P.h"
Throwbot 0:603c28b75dc1 7
Throwbot 0:603c28b75dc1 8 #define BaudRate_bt 9600 //Baud rate of 9600
Throwbot 0:603c28b75dc1 9 #define tx_bt PTA2 //Bluetooth connection pins
Throwbot 0:603c28b75dc1 10 #define rx_bt PTA1 //Bluetooth connection pins
Throwbot 0:603c28b75dc1 11 #define tx_mpu PTE0 //MPU connection pins
Throwbot 0:603c28b75dc1 12 #define rx_mpu PTE1 //MPU connection pins
Throwbot 0:603c28b75dc1 13 #define mpu_bandwidth MPU6050_BW_20 //set the MPU low pass filter bandwidth to 20hz
Throwbot 0:603c28b75dc1 14
Throwbot 0:603c28b75dc1 15 #define LED PTE3 //status LED pin
Throwbot 0:603c28b75dc1 16 #define CURRENTSENSOR_PIN PTC2
Throwbot 0:603c28b75dc1 17 #define VOLTAGESENSOR_PIN PTB0
Throwbot 0:603c28b75dc1 18
Throwbot 0:603c28b75dc1 19 #define CURRENT_R1 180 //160.0 //values of the current sensor opamp resistors
Throwbot 0:603c28b75dc1 20 #define CURRENT_R2 10
Throwbot 0:603c28b75dc1 21 #define CURRENT_R3 80
Throwbot 0:603c28b75dc1 22 #define CURRENT_R4 84.7
Throwbot 0:603c28b75dc1 23 #define VREF3_3 3.3 //digital logic voltage
Throwbot 0:603c28b75dc1 24 #define VREF5 5.0 //5v voltage //NOTE: 5v voltage is consistent when using new batts, but not using old blue batts
Throwbot 0:603c28b75dc1 25
Throwbot 0:603c28b75dc1 26 #define irL PTB1
Throwbot 0:603c28b75dc1 27 #define irC PTB3
Throwbot 0:603c28b75dc1 28 #define irR PTB2
Throwbot 0:603c28b75dc1 29
Throwbot 0:603c28b75dc1 30 #define MOT_PWMA_PIN PTA4 //Motor control pins
Throwbot 0:603c28b75dc1 31 #define MOT_PWMB_PIN PTA5
Throwbot 0:603c28b75dc1 32 #define MOT_STBY_PIN PTA15
Throwbot 0:603c28b75dc1 33 #define MOT_AIN1_PIN PTA14
Throwbot 0:603c28b75dc1 34 #define MOT_AIN2_PIN PTA13
Throwbot 0:603c28b75dc1 35 #define MOT_BIN1_PIN PTA16
Throwbot 0:603c28b75dc1 36 #define MOT_BIN2_PIN PTA17
Throwbot 0:603c28b75dc1 37
Throwbot 0:603c28b75dc1 38 #define M_PI 3.14159265359 //PI
Throwbot 0:603c28b75dc1 39 #define gyroCorrect 3720 //divide raw gyro data by this to get result in RAD/SECOND (if gyroRange is 500 rad/s)
Throwbot 0:603c28b75dc1 40
Throwbot 0:603c28b75dc1 41 //Correct direction of motors. If number = 1, forward. If number = 0, backwards (for if motors are wired backwards)
Throwbot 0:603c28b75dc1 42 #define MOTOR_R_DIRECTION 1
Throwbot 0:603c28b75dc1 43 #define MOTOR_L_DIRECTION 1
Throwbot 0:603c28b75dc1 44
Throwbot 0:603c28b75dc1 45 #define MOTOR_INTERVAL 20 //defines the interval (in milliseconds) between when motor can be set
Throwbot 0:603c28b75dc1 46 //NOTE: Can't make this less than 20ms, because that is the PWM frequency
Throwbot 0:603c28b75dc1 47
Throwbot 0:603c28b75dc1 48 //Key bindings for remote control robot - for the future try to use arrow keys instead of 'asdw'
Throwbot 0:603c28b75dc1 49 #define ctrl_forward 'i' //forward
Throwbot 0:603c28b75dc1 50 #define ctrl_backward 'k' //back
Throwbot 0:603c28b75dc1 51 #define ctrl_left 'j' //turn left
Throwbot 0:603c28b75dc1 52 #define ctrl_right 'l' //turn right
Throwbot 0:603c28b75dc1 53 #define ctrl_calibrate 'c' //re-calibrate the accelerometer and gyro
Throwbot 0:603c28b75dc1 54 #define ctrl_turn_angle_cw 'o' // turn angle
Throwbot 0:603c28b75dc1 55 #define ctrl_turn_angle_ccw 'p'
Throwbot 0:603c28b75dc1 56 // The nRF24L01+ supports transfers from 1 to 32 bytes, but Sparkfun's
Throwbot 0:603c28b75dc1 57 // "Nordic Serial Interface Board" (http://www.sparkfun.com/products/9019)
Throwbot 0:603c28b75dc1 58 // only handles 4 byte transfers in the ATMega code.
Throwbot 0:603c28b75dc1 59 #define TRANSFER_SIZE 4
Throwbot 0:603c28b75dc1 60
Throwbot 0:603c28b75dc1 61 class Robot
Throwbot 0:603c28b75dc1 62 {
Throwbot 0:603c28b75dc1 63 public:
Throwbot 0:603c28b75dc1 64 /**
Throwbot 0:603c28b75dc1 65 * Constructor - does nothing atm
Throwbot 0:603c28b75dc1 66 */
Throwbot 0:603c28b75dc1 67 Robot();
Throwbot 0:603c28b75dc1 68
Throwbot 0:603c28b75dc1 69 /**
Throwbot 0:603c28b75dc1 70 * MOTOR CONTROLS
Throwbot 0:603c28b75dc1 71 */
Throwbot 0:603c28b75dc1 72 void motor_control(int Lspeed, int Rspeed); //Input speed for motors. Integer between 0 and 100
Throwbot 0:603c28b75dc1 73 void stop(); //stop motors
Throwbot 0:603c28b75dc1 74 void set_direction(double angle); //set angle for the robot to face (from origin)
Throwbot 0:603c28b75dc1 75 void set_direction_deg(double angle);
Throwbot 0:603c28b75dc1 76 void set_speed(int Speed ); //set speed for robot to travel at
Throwbot 0:603c28b75dc1 77 void auto_enable(bool x);
Throwbot 0:603c28b75dc1 78 /**
Throwbot 0:603c28b75dc1 79 * MPU CONTROLS
Throwbot 0:603c28b75dc1 80 */
Throwbot 0:603c28b75dc1 81
Throwbot 0:603c28b75dc1 82 /**
Throwbot 0:603c28b75dc1 83 * UPDATE (this must be called repeatedly so the robot will sample accelerometer to find position)
Throwbot 0:603c28b75dc1 84 */
Throwbot 0:603c28b75dc1 85 void update(); //print variable decides which values to print
Throwbot 0:603c28b75dc1 86 void remote_ctrl(char c,int desired_speed, int desired_angle);
Throwbot 0:603c28b75dc1 87 //print = 0: nothing
Throwbot 0:603c28b75dc1 88 //print = 1: Accelerometer and gyro
Throwbot 0:603c28b75dc1 89 //print = 2: Current and voltage
Throwbot 0:603c28b75dc1 90
Throwbot 0:603c28b75dc1 91 // calibrate the gyro and accelerometer //
Throwbot 0:603c28b75dc1 92 void calibrate();
Throwbot 0:603c28b75dc1 93
Throwbot 0:603c28b75dc1 94 /**
Throwbot 0:603c28b75dc1 95 * Status: find the distance, orientation, battery values, etc of the robot
Throwbot 0:603c28b75dc1 96 */
Throwbot 0:603c28b75dc1 97 //void distanceTravelled(double x[3])
Throwbot 0:603c28b75dc1 98 //void orientation(something quaternion? on xy plane?)
Throwbot 0:603c28b75dc1 99 double getCurrent(); //Get the current drawn by the robot
Throwbot 0:603c28b75dc1 100 double getCurrent(int n); //get the current, averaged over n samples
Throwbot 0:603c28b75dc1 101 double getVoltage(); //get the battery voltage (ask connor for completed function)
Throwbot 0:603c28b75dc1 102 void getIMU(float *adata, float *gdata);
Throwbot 0:603c28b75dc1 103 double irReadL();
Throwbot 0:603c28b75dc1 104 double irReadC();
Throwbot 0:603c28b75dc1 105 double irReadR();
Throwbot 0:603c28b75dc1 106 double return_rotation();
Throwbot 0:603c28b75dc1 107 int isMPU();
Throwbot 0:603c28b75dc1 108
Throwbot 0:603c28b75dc1 109
Throwbot 0:603c28b75dc1 110 //Wireless connections
Throwbot 0:603c28b75dc1 111 Serial bt; //bluetooth connection
Throwbot 0:603c28b75dc1 112 nRF24L01P rf24; //RF network connection
Throwbot 0:603c28b75dc1 113
Throwbot 0:603c28b75dc1 114 //RF24 network functions//
Throwbot 0:603c28b75dc1 115 void rf24_power(int status); //power on or off the RF network
Throwbot 0:603c28b75dc1 116 char rf24_read();
Throwbot 0:603c28b75dc1 117 int rf24_write(char letter); //write a letter to RF
Throwbot 0:603c28b75dc1 118 int rf24_write(char* buffer, int length); //write
Throwbot 0:603c28b75dc1 119 int acc[3];
Throwbot 0:603c28b75dc1 120 int gyr[3];
Throwbot 0:603c28b75dc1 121
Throwbot 0:603c28b75dc1 122
Throwbot 0:603c28b75dc1 123
Throwbot 0:603c28b75dc1 124 //-------------------PRIVATE VARIABLES AND FUNCTIONS-----------------------------------------------//
Throwbot 0:603c28b75dc1 125 private:
Throwbot 0:603c28b75dc1 126
Throwbot 0:603c28b75dc1 127 //commands for remote control robot
Throwbot 0:603c28b75dc1 128
Throwbot 0:603c28b75dc1 129 MPU6050 mpu; //MPU connection
Throwbot 0:603c28b75dc1 130 DigitalOut myled; //(PTE3) Processor LED (1 = off, 0 = on)
Throwbot 0:603c28b75dc1 131 DigitalOut btSwitch;
Throwbot 0:603c28b75dc1 132 AnalogIn currentSensor;
Throwbot 0:603c28b75dc1 133 AnalogIn irSensorL;
Throwbot 0:603c28b75dc1 134 AnalogIn irSensorC;
Throwbot 0:603c28b75dc1 135 AnalogIn irSensorR;
Throwbot 0:603c28b75dc1 136 AnalogIn voltageSensor;
Throwbot 0:603c28b75dc1 137
Throwbot 0:603c28b75dc1 138 double dx; //distance travelled in x direction
Throwbot 0:603c28b75dc1 139 double dy; //distance travelled in y direction
Throwbot 0:603c28b75dc1 140 double dz; //distance travelled in z direction (zero?)
Throwbot 0:603c28b75dc1 141 double origin; //location of robot origin (or can be set if robot starting location is known.
Throwbot 0:603c28b75dc1 142 double target_angle; //direction that we want the robot to face (radians)
Throwbot 0:603c28b75dc1 143
Throwbot 0:603c28b75dc1 144 int accdata[3]; //data from accelerometer (raw)
Throwbot 0:603c28b75dc1 145 int gyrodata[3]; //data from gyro (raw)
Throwbot 0:603c28b75dc1 146 //double gyroCorrect; //= 3720; //divide by this to get gyro data in RAD/SECOND. This is above as a #DEFINE
Throwbot 0:603c28b75dc1 147 int gyroOffset[3]; //Correction value for each gyroscope to zero the values.
Throwbot 0:603c28b75dc1 148 int accOffset[3]; //correction value for each accelerometer
Throwbot 0:603c28b75dc1 149 int speed; //set the speed of robot
Throwbot 0:603c28b75dc1 150 /**Angle is always measured in clockwise direction, looking down from the top**/
Throwbot 0:603c28b75dc1 151 /*Angle is measured in RADIANS*/
Throwbot 0:603c28b75dc1 152 double rz; //Direction robot is facing
Throwbot 0:603c28b75dc1 153 double Irz; //integral of the rotation offset from target. (Optionally) Used for PID control of direction
Throwbot 0:603c28b75dc1 154 double angle_origin; //Angle of origin (can be changed later, or set if robot starts at known angle)
Throwbot 0:603c28b75dc1 155 bool AUTO_ORIENT; //if this flag is 1, the robot automatically orients itself to selected direction
Throwbot 0:603c28b75dc1 156 bool REMOTE_CONTROL; //if this flag is 1, the robot will be controlled over bluetooth
Throwbot 0:603c28b75dc1 157
Throwbot 0:603c28b75dc1 158 /////////// Motor control variables ///////////
Throwbot 0:603c28b75dc1 159 PwmOut PWMA;//(MOT_PWMA_PIN);
Throwbot 0:603c28b75dc1 160 PwmOut PWMB;//(MOT_PWMB_PIN);
Throwbot 0:603c28b75dc1 161 DigitalOut AIN1;//(MOT_AIN1_PIN);
Throwbot 0:603c28b75dc1 162 DigitalOut AIN2;//(MOT_AIN2_PIN);
Throwbot 0:603c28b75dc1 163 DigitalOut BIN1;//(MOT_BIN1_PIN);
Throwbot 0:603c28b75dc1 164 DigitalOut BIN2;//(MOT_BIN2_PIN);
Throwbot 0:603c28b75dc1 165 DigitalOut STBY;//(MOT_STBY_PIN);
Throwbot 0:603c28b75dc1 166 double timeNext; //next time that the motor is allowed to be updated
Throwbot 0:603c28b75dc1 167
Throwbot 0:603c28b75dc1 168 bool MPU_OK;
Throwbot 0:603c28b75dc1 169
Throwbot 0:603c28b75dc1 170 Timer tt; //timer
Throwbot 0:603c28b75dc1 171
Throwbot 0:603c28b75dc1 172 double time; //time of current iteration
Throwbot 0:603c28b75dc1 173 double timePrev; //time of previous iteration
Throwbot 0:603c28b75dc1 174
Throwbot 0:603c28b75dc1 175
Throwbot 0:603c28b75dc1 176 };
Throwbot 0:603c28b75dc1 177 #endif