Evo Bot / Robot

BT control

robot.cpp@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 /* mbed ROBOT Library, for SUTD evobot project, Generation 1
Throwbot 0:603c28b75dc1 2 * Copyright (c) 2013, SUTD
Throwbot 0:603c28b75dc1 3 * Author: Mark VanderMeulen
Throwbot 0:603c28b75dc1 4 *
Throwbot 0:603c28b75dc1 5 * Dec 18, 2013
Throwbot 0:603c28b75dc1 6 *
Throwbot 0:603c28b75dc1 7 * You may want to rewrite this entire library, as this one is only half-finished
Throwbot 0:603c28b75dc1 8 *
Throwbot 0:603c28b75dc1 9 * This library allows the user to use each module on the Evobot Generation 1
Throwbot 0:603c28b75dc1 10 * Functionality:
Throwbot 0:603c28b75dc1 11 * -Bluetooth connection
Throwbot 0:603c28b75dc1 12 * -MPU (accelerometer/gyroscope) connection
Throwbot 0:603c28b75dc1 13 * -Orientation in the XY plane (using Z-axis gyroscope only)
Throwbot 0:603c28b75dc1 14 * -Remote control functionality (via bluetooth)
Throwbot 0:603c28b75dc1 15 *
Throwbot 0:603c28b75dc1 16 * Future functions:
Throwbot 0:603c28b75dc1 17 * -Switch to turn bluetooth on/off
Throwbot 0:603c28b75dc1 18 * -Switch to turn MPU on/off (in case of connection errors with the MPU: restart and reconnect
Throwbot 0:603c28b75dc1 19 * -Calculation of distance travelled (use integration of accelerometer, maybe use Kalman filter)
Throwbot 0:603c28b75dc1 20 * -Access to the DMP (Digital motion processor) on the MPU to calculate 3D orientation.
Throwbot 0:603c28b75dc1 21 * -RF mesh network so robots can connect to eachother
Throwbot 0:603c28b75dc1 22 * -Camera (requires an add-on to the circuit board at the moment)
Throwbot 0:603c28b75dc1 23 */
Throwbot 0:603c28b75dc1 24
Throwbot 0:603c28b75dc1 25
Throwbot 0:603c28b75dc1 26 #include "robot.h"
Throwbot 0:603c28b75dc1 27 #include "math.h"
Throwbot 0:603c28b75dc1 28 //*********************************CONSTRUCTOR*********************************//
Throwbot 0:603c28b75dc1 29 Robot::Robot() : bt(tx_bt,rx_bt),
Throwbot 0:603c28b75dc1 30 rf24(PTD2, PTD3, PTD1, PTD0, PTD5, PTD4),
Throwbot 0:603c28b75dc1 31 mpu(PTE0, PTE1),
Throwbot 0:603c28b75dc1 32 myled(LED),
Throwbot 0:603c28b75dc1 33 btSwitch(PTE25),
Throwbot 0:603c28b75dc1 34 currentSensor(CURRENTSENSOR_PIN),
Throwbot 0:603c28b75dc1 35 irSensorL(irL),
Throwbot 0:603c28b75dc1 36 irSensorC(irC),
Throwbot 0:603c28b75dc1 37 irSensorR(irR),
Throwbot 0:603c28b75dc1 38 voltageSensor(VOLTAGESENSOR_PIN),
Throwbot 0:603c28b75dc1 39 PWMA(MOT_PWMA_PIN),
Throwbot 0:603c28b75dc1 40 PWMB(MOT_PWMB_PIN),
Throwbot 0:603c28b75dc1 41 AIN1(MOT_AIN1_PIN),
Throwbot 0:603c28b75dc1 42 AIN2(MOT_AIN2_PIN),
Throwbot 0:603c28b75dc1 43 BIN1(MOT_BIN1_PIN),
Throwbot 0:603c28b75dc1 44 BIN2(MOT_BIN2_PIN),
Throwbot 0:603c28b75dc1 45 STBY(MOT_STBY_PIN)
Throwbot 0:603c28b75dc1 46 {
Throwbot 0:603c28b75dc1 47
Throwbot 0:603c28b75dc1 48 stop(); //Initialize motors as stopped//
Throwbot 0:603c28b75dc1 49
Throwbot 0:603c28b75dc1 50 btSwitch = 1; //turn on bluetooth
Throwbot 0:603c28b75dc1 51 myled = 0; //turn ON status LED (0 = on, 1 = 0ff)
Throwbot 0:603c28b75dc1 52 timeNext = 0;
Throwbot 0:603c28b75dc1 53 //currentAvg = 0; /////////////REMOVE currentAvg LATER. TESTING PURPOSES ONLY////////
Throwbot 0:603c28b75dc1 54
Throwbot 0:603c28b75dc1 55 target_angle = 0; //direction we want the robot to be pointed
Throwbot 0:603c28b75dc1 56 angle_origin = 0; //you can use this to modify the angle of origin
Throwbot 0:603c28b75dc1 57 origin = 0; //the (x,y) location of the origin (this should be a point, not a double)
Throwbot 0:603c28b75dc1 58 rz = 0; //The current rotation in the Z-axis
Throwbot 0:603c28b75dc1 59 dx = 0; //The current displacement in the x-axis (side-side)
Throwbot 0:603c28b75dc1 60 dy = 0; //The current displacement in the y-axis (forward-back)
Throwbot 0:603c28b75dc1 61 dz = 0; //The current displacement in the z-axis (up-down)
Throwbot 0:603c28b75dc1 62
Throwbot 0:603c28b75dc1 63 AUTO_ORIENT = 1; //robot will automatically orient iteslf using the gyroscope
Throwbot 0:603c28b75dc1 64 REMOTE_CONTROL = 1; //robot can be controlled over bluetooth
Throwbot 0:603c28b75dc1 65
Throwbot 0:603c28b75dc1 66 tt.start(); //start timer
Throwbot 0:603c28b75dc1 67 mpu.testConnection();
Throwbot 0:603c28b75dc1 68 wait(1);
Throwbot 0:603c28b75dc1 69 MPU_OK = 0;
Throwbot 0:603c28b75dc1 70
Throwbot 0:603c28b75dc1 71 //initialize MPU
Throwbot 0:603c28b75dc1 72 if (mpu.testConnection()) {
Throwbot 0:603c28b75dc1 73 mpu.setBW(MPU6050_BW_10); //default set low pass filter bandwidth to 10HZ
Throwbot 0:603c28b75dc1 74 mpu.setGyroRange(MPU6050_GYRO_RANGE_500); //default set the gyro range to 500deg/s (robot turning exceeds 250deg/s)
Throwbot 0:603c28b75dc1 75 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); //default set accelero range to 2g
Throwbot 0:603c28b75dc1 76 MPU_OK=1;
Throwbot 0:603c28b75dc1 77 myled = 0; //turn on LED
Throwbot 0:603c28b75dc1 78 calibrate();
Throwbot 0:603c28b75dc1 79 } else if(0) { //this section is current disabled. change to (1) if you want to retry the accelerometer connection.
Throwbot 0:603c28b75dc1 80 myled = 1; //turn off LED
Throwbot 0:603c28b75dc1 81 for (int i = 0; i<25; i++) {
Throwbot 0:603c28b75dc1 82 myled = !myled;
Throwbot 0:603c28b75dc1 83 wait_ms(50);
Throwbot 0:603c28b75dc1 84 if (mpu.testConnection()) {
Throwbot 0:603c28b75dc1 85 i = 25;
Throwbot 0:603c28b75dc1 86 myled = 0;
Throwbot 0:603c28b75dc1 87 MPU_OK=1;
Throwbot 0:603c28b75dc1 88 } else
Throwbot 0:603c28b75dc1 89 myled = 1;
Throwbot 0:603c28b75dc1 90 wait_ms(50);
Throwbot 0:603c28b75dc1 91 }
Throwbot 0:603c28b75dc1 92 }
Throwbot 0:603c28b75dc1 93 myled = MPU_OK; //If LED is off, it is ok. If LED is on, there was a MPU error. Board needs to be restarted.
Throwbot 0:603c28b75dc1 94 //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.
Throwbot 0:603c28b75dc1 95
Throwbot 0:603c28b75dc1 96 bt.baud(BaudRate_bt); //Set bluetooth baud rate
Throwbot 0:603c28b75dc1 97
Throwbot 0:603c28b75dc1 98 //Initialize RF chip
Throwbot 0:603c28b75dc1 99 //char txData[TRANSFER_SIZE], rxData[TRANSFER_SIZE];
Throwbot 0:603c28b75dc1 100 // int txDataCnt = 0;
Throwbot 0:603c28b75dc1 101 // int rxDataCnt = 0;
Throwbot 0:603c28b75dc1 102 rf24.powerUp();
Throwbot 0:603c28b75dc1 103 rf24.setTransferSize( TRANSFER_SIZE );
Throwbot 0:603c28b75dc1 104
Throwbot 0:603c28b75dc1 105 rf24.setReceiveMode();
Throwbot 0:603c28b75dc1 106 rf24.enable();
Throwbot 0:603c28b75dc1 107
Throwbot 0:603c28b75dc1 108
Throwbot 0:603c28b75dc1 109
Throwbot 0:603c28b75dc1 110 }
Throwbot 0:603c28b75dc1 111
Throwbot 0:603c28b75dc1 112 //*********************************MOTORS*********************************//
Throwbot 0:603c28b75dc1 113 void Robot::motor_control(int Lspeed, int Rspeed)
Throwbot 0:603c28b75dc1 114 {
Throwbot 0:603c28b75dc1 115 //Controls the motors. 0 = stopped, 100 = full speed, -100 = reverse speed
Throwbot 0:603c28b75dc1 116 if (!Lspeed && !Rspeed) //stop//
Throwbot 0:603c28b75dc1 117 STBY = 0;
Throwbot 0:603c28b75dc1 118 else
Throwbot 0:603c28b75dc1 119 STBY = 1;
Throwbot 0:603c28b75dc1 120
Throwbot 0:603c28b75dc1 121 //make sure 'speeds' are between -100 and 100, and not between abs(0 to 10)
Throwbot 0:603c28b75dc1 122 if(Lspeed > 100) Lspeed = 100;
Throwbot 0:603c28b75dc1 123 else if (Lspeed < -100) Lspeed = -100;
Throwbot 0:603c28b75dc1 124 else if (Lspeed < 0 && Lspeed > -15) Lspeed = -15; //prevent speed from being less than 15
Throwbot 0:603c28b75dc1 125 else if (Lspeed > 0 && Lspeed < 15) Lspeed = 15;
Throwbot 0:603c28b75dc1 126 if(Rspeed > 100) Rspeed = 100;
Throwbot 0:603c28b75dc1 127 else if (Rspeed < -100) Rspeed = -100;
Throwbot 0:603c28b75dc1 128 else if (Rspeed < 0 && Rspeed > -15) Rspeed = -15;
Throwbot 0:603c28b75dc1 129 else if (Rspeed > 0 && Rspeed < 15) Rspeed = 15;
Throwbot 0:603c28b75dc1 130
Throwbot 0:603c28b75dc1 131 if (!Rspeed) { //if right motor is stopped
Throwbot 0:603c28b75dc1 132 AIN1 = 0;
Throwbot 0:603c28b75dc1 133 AIN2 = 0;
Throwbot 0:603c28b75dc1 134 PWMA = 0;
Throwbot 0:603c28b75dc1 135 } else if (!Lspeed) { //if left motor is stopped
Throwbot 0:603c28b75dc1 136 BIN1 = 0;
Throwbot 0:603c28b75dc1 137 BIN2 = 0;
Throwbot 0:603c28b75dc1 138 PWMB = 0;
Throwbot 0:603c28b75dc1 139 }
Throwbot 0:603c28b75dc1 140 //RIGHT MOTOR//
Throwbot 0:603c28b75dc1 141 if(Rspeed > 0) { //Right motor fwd
Throwbot 0:603c28b75dc1 142 AIN1 = MOTOR_R_DIRECTION; //set the motor direction
Throwbot 0:603c28b75dc1 143 AIN2 = !AIN1;
Throwbot 0:603c28b75dc1 144 } else { //Right motor reverse
Throwbot 0:603c28b75dc1 145 AIN2 = MOTOR_R_DIRECTION;
Throwbot 0:603c28b75dc1 146 AIN1 = !AIN2;
Throwbot 0:603c28b75dc1 147 }
Throwbot 0:603c28b75dc1 148 PWMA = abs(Rspeed/100.0);
Throwbot 0:603c28b75dc1 149 //LEFT MOTOR//
Throwbot 0:603c28b75dc1 150 if(Lspeed >0) {
Throwbot 0:603c28b75dc1 151 BIN1 = MOTOR_L_DIRECTION;
Throwbot 0:603c28b75dc1 152 BIN2 = !BIN1;
Throwbot 0:603c28b75dc1 153 } else {
Throwbot 0:603c28b75dc1 154 BIN2 = MOTOR_L_DIRECTION;
Throwbot 0:603c28b75dc1 155 BIN1 = !BIN2;
Throwbot 0:603c28b75dc1 156 }
Throwbot 0:603c28b75dc1 157 PWMB = abs(Lspeed/100.0);
Throwbot 0:603c28b75dc1 158 }
Throwbot 0:603c28b75dc1 159
Throwbot 0:603c28b75dc1 160 void Robot::stop()
Throwbot 0:603c28b75dc1 161 {
Throwbot 0:603c28b75dc1 162 motor_control(0,0);
Throwbot 0:603c28b75dc1 163 }
Throwbot 0:603c28b75dc1 164 void Robot::set_speed(int Speed)
Throwbot 0:603c28b75dc1 165 {
Throwbot 0:603c28b75dc1 166 speed = Speed;
Throwbot 0:603c28b75dc1 167 motor_control(speed,speed);
Throwbot 0:603c28b75dc1 168 }
Throwbot 0:603c28b75dc1 169
Throwbot 0:603c28b75dc1 170 //************************Setting direction of robot****************************//
Throwbot 0:603c28b75dc1 171 void Robot::set_direction(double angle)
Throwbot 0:603c28b75dc1 172 {
Throwbot 0:603c28b75dc1 173 //Set the direction the robot should be facing from origin(in radians)
Throwbot 0:603c28b75dc1 174 target_angle = angle;
Throwbot 0:603c28b75dc1 175 }
Throwbot 0:603c28b75dc1 176 void Robot::set_direction_deg(double angle)
Throwbot 0:603c28b75dc1 177 {
Throwbot 0:603c28b75dc1 178 //Set the direction the robot should be facing from origin (in degrees)
Throwbot 0:603c28b75dc1 179 target_angle = angle*M_PI/180;
Throwbot 0:603c28b75dc1 180 }
Throwbot 0:603c28b75dc1 181 void Robot::auto_enable(bool x)
Throwbot 0:603c28b75dc1 182 {
Throwbot 0:603c28b75dc1 183 AUTO_ORIENT = x;
Throwbot 0:603c28b75dc1 184 }
Throwbot 0:603c28b75dc1 185
Throwbot 0:603c28b75dc1 186 //************************UPDATE: To be called in main loop***************************//
Throwbot 0:603c28b75dc1 187 //This calculates position and angle
Throwbot 0:603c28b75dc1 188 //Also orients robot in the correct direction
Throwbot 0:603c28b75dc1 189 //if 'print' is 1, print acceleration and gyro data over bluetooth
Throwbot 0:603c28b75dc1 190 void Robot::update()
Throwbot 0:603c28b75dc1 191 {
Throwbot 0:603c28b75dc1 192 double SP;
Throwbot 0:603c28b75dc1 193 int rotationSpeed;
Throwbot 0:603c28b75dc1 194 //int n=50;
Throwbot 0:603c28b75dc1 195
Throwbot 0:603c28b75dc1 196 double Kp = 100; //weighting values for PID controller. Only Proportional component is used.
Throwbot 0:603c28b75dc1 197 double Ki = 0; //Integral component is not used
Throwbot 0:603c28b75dc1 198 double Kd = 0; //derivative component is not used
Throwbot 0:603c28b75dc1 199
Throwbot 0:603c28b75dc1 200 if(MPU_OK) { //only do this if MPU is connected
Throwbot 0:603c28b75dc1 201 wait(0.1);
Throwbot 0:603c28b75dc1 202 mpu.getAcceleroRaw(accdata);
Throwbot 0:603c28b75dc1 203 mpu.getGyroRaw(gyrodata);
Throwbot 0:603c28b75dc1 204 time = tt.read();
Throwbot 0:603c28b75dc1 205 //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());
Throwbot 0:603c28b75dc1 206 /*if(REMOTE_CONTROL) { //if remote control over bluetooth is enabled
Throwbot 0:603c28b75dc1 207 m_ctrl=remote_ctrl(); //call the romote control function
Throwbot 0:603c28b75dc1 208 }*/
Throwbot 0:603c28b75dc1 209 if(AUTO_ORIENT) { //enable PID control of angle
Throwbot 0:603c28b75dc1 210 SP = target_angle; //desired angle (radians)
Throwbot 0:603c28b75dc1 211 rz = rz + ((gyrodata[2]-gyroOffset[2])*(time-timePrev)/gyroCorrect); //rz is the rotation(radians) from start
Throwbot 0:603c28b75dc1 212 Irz = Irz + (rz-SP)*(time-timePrev); //Irz needs to be reset every so often, or it should be ignored
Throwbot 0:603c28b75dc1 213 rotationSpeed = (Kp*(rz-SP) + Ki*Irz + Kd*gyrodata[2]/gyroCorrect);
Throwbot 0:603c28b75dc1 214 /*data[0]= getCurrent();
Throwbot 0:603c28b75dc1 215 data[1]= getVoltage();
Throwbot 0:603c28b75dc1 216 data[2]= accdata[0];
Throwbot 0:603c28b75dc1 217 data[3]= accdata[1];
Throwbot 0:603c28b75dc1 218 data[4]= accdata[2];
Throwbot 0:603c28b75dc1 219 data[5]= gyrodata[0];
Throwbot 0:603c28b75dc1 220 data[6]= gyrodata[1];
Throwbot 0:603c28b75dc1 221 data[7]= gyrodata[2];
Throwbot 0:603c28b75dc1 222 data[8]= irReadL();
Throwbot 0:603c28b75dc1 223 data[9]= irReadC();
Throwbot 0:603c28b75dc1 224 data[10]= irReadC();
Throwbot 0:603c28b75dc1 225 data[11]= rz;*/
Throwbot 0:603c28b75dc1 226
Throwbot 0:603c28b75dc1 227 //TODO: pull "rotationspeed" up to 10 if it is less than 10. This will(should) improve the ability to drive straight
Throwbot 0:603c28b75dc1 228
Throwbot 0:603c28b75dc1 229 if(time > timeNext || (speed==0 && rotationSpeed ==0)) { //prevent the motor control from being set too often
Throwbot 0:603c28b75dc1 230 timeNext = time + MOTOR_INTERVAL/1000.0; //only set the motor speed every 10ms
Throwbot 0:603c28b75dc1 231 // if(abs(rz-SP)>(10*M_PI/180))
Throwbot 0:603c28b75dc1 232 // {
Throwbot 0:603c28b75dc1 233 // motor_control((speed+rotationSpeed)/20, (speed-rotationSpeed)/20); //Set motor speeds
Throwbot 0:603c28b75dc1 234 //}
Throwbot 0:603c28b75dc1 235 // else
Throwbot 0:603c28b75dc1 236 //{
Throwbot 0:603c28b75dc1 237 motor_control((speed+rotationSpeed), (speed-rotationSpeed)); //Set motor speeds
Throwbot 0:603c28b75dc1 238 //}
Throwbot 0:603c28b75dc1 239 }
Throwbot 0:603c28b75dc1 240 }
Throwbot 0:603c28b75dc1 241 timePrev = time;
Throwbot 0:603c28b75dc1 242 }
Throwbot 0:603c28b75dc1 243 }
Throwbot 0:603c28b75dc1 244
Throwbot 0:603c28b75dc1 245 void Robot::remote_ctrl(char c,int desired_speed,int desired_angle)
Throwbot 0:603c28b75dc1 246 {
Throwbot 0:603c28b75dc1 247 //This is a private function
Throwbot 0:603c28b75dc1 248 Irz = 0;
Throwbot 0:603c28b75dc1 249 switch (c) {
Throwbot 0:603c28b75dc1 250 case ctrl_forward:
Throwbot 0:603c28b75dc1 251 speed = -1*desired_speed;
Throwbot 0:603c28b75dc1 252 break;
Throwbot 0:603c28b75dc1 253 case ctrl_backward:
Throwbot 0:603c28b75dc1 254 speed = 1*desired_speed;
Throwbot 0:603c28b75dc1 255 break;
Throwbot 0:603c28b75dc1 256 case ctrl_right:
Throwbot 0:603c28b75dc1 257 target_angle += 90*M_PI/180;
Throwbot 0:603c28b75dc1 258 break;
Throwbot 0:603c28b75dc1 259 case ctrl_left:
Throwbot 0:603c28b75dc1 260 target_angle -= 90*M_PI/180;
Throwbot 0:603c28b75dc1 261 break;
Throwbot 0:603c28b75dc1 262 case ctrl_calibrate:
Throwbot 0:603c28b75dc1 263 calibrate();
Throwbot 0:603c28b75dc1 264 break;
Throwbot 0:603c28b75dc1 265 case ctrl_turn_angle_cw:
Throwbot 0:603c28b75dc1 266 target_angle +=desired_angle*M_PI/180;
Throwbot 0:603c28b75dc1 267 break;
Throwbot 0:603c28b75dc1 268 case ctrl_turn_angle_ccw:
Throwbot 0:603c28b75dc1 269 target_angle -=desired_angle*M_PI/180;
Throwbot 0:603c28b75dc1 270 break;
Throwbot 0:603c28b75dc1 271 default:
Throwbot 0:603c28b75dc1 272 speed = 0;
Throwbot 0:603c28b75dc1 273 stop();
Throwbot 0:603c28b75dc1 274 break;
Throwbot 0:603c28b75dc1 275 }
Throwbot 0:603c28b75dc1 276 }
Throwbot 0:603c28b75dc1 277 //*********************************CALIBRATE*********************************//
Throwbot 0:603c28b75dc1 278 void Robot::calibrate()
Throwbot 0:603c28b75dc1 279 {
Throwbot 0:603c28b75dc1 280 stop();
Throwbot 0:603c28b75dc1 281 wait(1.5);
Throwbot 0:603c28b75dc1 282 double timeNOW = tt.read();
Throwbot 0:603c28b75dc1 283 int count=0;
Throwbot 0:603c28b75dc1 284 int i;
Throwbot 0:603c28b75dc1 285
Throwbot 0:603c28b75dc1 286 //set the accelerometer and gyro offsets
Throwbot 0:603c28b75dc1 287 while(tt.read()<timeNOW+1.5) { //calculate gyro offset
Throwbot 0:603c28b75dc1 288 mpu.getGyroRaw(gyrodata);
Throwbot 0:603c28b75dc1 289 mpu.getAcceleroRaw(accdata);
Throwbot 0:603c28b75dc1 290 for(i=0; i<3; i++) {
Throwbot 0:603c28b75dc1 291 gyroOffset[i] += gyrodata[i];
Throwbot 0:603c28b75dc1 292 accOffset[i] += accdata[i];
Throwbot 0:603c28b75dc1 293 }
Throwbot 0:603c28b75dc1 294 count++;
Throwbot 0:603c28b75dc1 295 }
Throwbot 0:603c28b75dc1 296 for(i=0; i<3; i++) {
Throwbot 0:603c28b75dc1 297 gyroOffset[i] = gyroOffset[i]/count; //rxOffset
Throwbot 0:603c28b75dc1 298 accOffset[i] = accOffset[i]/count;
Throwbot 0:603c28b75dc1 299 }
Throwbot 0:603c28b75dc1 300 accOffset[2] = 0; //we don't want to remove GRAVITY from the z-axis accelerometer.
Throwbot 0:603c28b75dc1 301 }
Throwbot 0:603c28b75dc1 302
Throwbot 0:603c28b75dc1 303 //*********************************ROBOT-SENSORS*********************************//
Throwbot 0:603c28b75dc1 304
Throwbot 0:603c28b75dc1 305 double Robot::getCurrent()
Throwbot 0:603c28b75dc1 306 {
Throwbot 0:603c28b75dc1 307
Throwbot 0:603c28b75dc1 308 double Vsensor = currentSensor.read();
Throwbot 0:603c28b75dc1 309 Vsensor = 1000*((.8998-Vsensor)/.6411);
Throwbot 0:603c28b75dc1 310 return Vsensor;
Throwbot 0:603c28b75dc1 311 }
Throwbot 0:603c28b75dc1 312 double Robot::getVoltage()
Throwbot 0:603c28b75dc1 313 {
Throwbot 0:603c28b75dc1 314 float voltage = 3.3*(voltageSensor.read()); // convert analog value to voltage at node
Throwbot 0:603c28b75dc1 315 voltage *= 1.5; // inverse of voltage divider
Throwbot 0:603c28b75dc1 316 return voltage;
Throwbot 0:603c28b75dc1 317 }
Throwbot 0:603c28b75dc1 318 void Robot::getIMU(float *adata, float *gdata)
Throwbot 0:603c28b75dc1 319 {
Throwbot 0:603c28b75dc1 320 mpu.getAcceleroRaw(accdata);
Throwbot 0:603c28b75dc1 321 mpu.getGyroRaw(gyrodata);
Throwbot 0:603c28b75dc1 322 /*imuFilter.updateFilter(gyrodata[0],gyrodata[1],gyrodata[2],accdata[0],accdata[1],accdata[2]);
Throwbot 0:603c28b75dc1 323 imuFilter.computeEuler();
Throwbot 0:603c28b75dc1 324 gdata[0]=(float)imuFilter.getRoll();
Throwbot 0:603c28b75dc1 325 gdata[1]=(float)imuFilter.getPitch();
Throwbot 0:603c28b75dc1 326 gdata[2]= (float)imuFilter.getYaw();*/
Throwbot 0:603c28b75dc1 327 // adata[0] = (float) (accdata[0]);
Throwbot 0:603c28b75dc1 328 //adata[1] = (float) (accdata[1]);
Throwbot 0:603c28b75dc1 329 //adata[2] = (float) (accdata[2]);
Throwbot 0:603c28b75dc1 330 adata[0] = ((float) (accdata[0]-accOffset[0]))/32655*9.81*2;
Throwbot 0:603c28b75dc1 331 adata[1]= ((float) (accdata[1]-accOffset[1]))/32655*9.81*2;
Throwbot 0:603c28b75dc1 332 adata[2]= ((float) (accdata[2]-accOffset[2]))/32655*9.81*2;
Throwbot 0:603c28b75dc1 333 gdata[0]=(((float)(gyrodata[0]-gyroOffset[0]))/((float)gyroCorrect))*180/(M_PI);
Throwbot 0:603c28b75dc1 334 gdata[1]=(((float)(gyrodata[1]-gyroOffset[1]))/((float)gyroCorrect))*180/(M_PI);
Throwbot 0:603c28b75dc1 335 gdata[2]=(((float)(gyrodata[2]-gyroOffset[2]))/((float)gyroCorrect))*180/(M_PI);
Throwbot 0:603c28b75dc1 336 }
Throwbot 0:603c28b75dc1 337
Throwbot 0:603c28b75dc1 338 double Robot::irReadL()
Throwbot 0:603c28b75dc1 339 {
Throwbot 0:603c28b75dc1 340 int C = 14;
Throwbot 0:603c28b75dc1 341 double D = -1.1;
Throwbot 0:603c28b75dc1 342 double r = irSensorL.read();
Throwbot 0:603c28b75dc1 343 double reading=(pow(r,D)* C )+ 4;
Throwbot 0:603c28b75dc1 344 return reading;
Throwbot 0:603c28b75dc1 345
Throwbot 0:603c28b75dc1 346 }
Throwbot 0:603c28b75dc1 347
Throwbot 0:603c28b75dc1 348 double Robot::irReadC()
Throwbot 0:603c28b75dc1 349 {
Throwbot 0:603c28b75dc1 350 int C = 14;
Throwbot 0:603c28b75dc1 351 double D = -1.1;
Throwbot 0:603c28b75dc1 352 double r = irSensorC.read();
Throwbot 0:603c28b75dc1 353 double reading=(pow(r,D)* C )+ 4;
Throwbot 0:603c28b75dc1 354 return reading;
Throwbot 0:603c28b75dc1 355 }
Throwbot 0:603c28b75dc1 356
Throwbot 0:603c28b75dc1 357 double Robot::irReadR()
Throwbot 0:603c28b75dc1 358 {
Throwbot 0:603c28b75dc1 359 int C = 14;
Throwbot 0:603c28b75dc1 360 double D = -1.1;
Throwbot 0:603c28b75dc1 361 double r = irSensorR.read();
Throwbot 0:603c28b75dc1 362 double reading=(pow(r,D)* C )+ 4;
Throwbot 0:603c28b75dc1 363 return reading;
Throwbot 0:603c28b75dc1 364 }
Throwbot 0:603c28b75dc1 365 double Robot::return_rotation()
Throwbot 0:603c28b75dc1 366 {
Throwbot 0:603c28b75dc1 367 return rz;
Throwbot 0:603c28b75dc1 368 }
Throwbot 0:603c28b75dc1 369 int Robot::isMPU()
Throwbot 0:603c28b75dc1 370 {
Throwbot 0:603c28b75dc1 371 return MPU_OK;
Throwbot 0:603c28b75dc1 372 }
Throwbot 0:603c28b75dc1 373
Throwbot 0:603c28b75dc1 374 //******************RF24 CHIP FUNCTIONS****************************//
Throwbot 0:603c28b75dc1 375 void Robot::rf24_power(int x)
Throwbot 0:603c28b75dc1 376 {
Throwbot 0:603c28b75dc1 377 if(x) //power up
Throwbot 0:603c28b75dc1 378 rf24.powerUp();
Throwbot 0:603c28b75dc1 379 else //power down
Throwbot 0:603c28b75dc1 380 rf24.powerDown();
Throwbot 0:603c28b75dc1 381 }