Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

robot/robot.cpp@0:21019d94ad33, 2015-03-21 (annotated)

Committer:
majik
Date:
Sat Mar 21 21:31:29 2015 +0000
Revision:
0:21019d94ad33
Child:
3:47461d37adfb
Both IMUs work now

Who changed what in which revision?

UserRevisionLine numberNew contents of line
majik 0:21019d94ad33 1 #include "robot.h"
majik 0:21019d94ad33 2 #include "bot08_cal.h"
majik 0:21019d94ad33 3
majik 0:21019d94ad33 4 /**Note: Initialize ALL UNUSED ANALOG IN pins as digital out to reduce noise on analog reads*//////
majik 0:21019d94ad33 5
majik 0:21019d94ad33 6 DigitalOut led(LED_PIN);
majik 0:21019d94ad33 7
majik 0:21019d94ad33 8 HC05 bt(TX_BT,RX_BT,EN_BT);
majik 0:21019d94ad33 9
majik 0:21019d94ad33 10 AnalogIn irL(IR_L);
majik 0:21019d94ad33 11 AnalogIn irC(IR_C);
majik 0:21019d94ad33 12 AnalogIn irR(IR_R);
majik 0:21019d94ad33 13 DigitalOut irBack(IR_B);
majik 0:21019d94ad33 14 AnalogIn voltage_sensor(VOLTAGESENSOR_PIN);
majik 0:21019d94ad33 15
majik 0:21019d94ad33 16 ACS712 current_sensor;
majik 0:21019d94ad33 17 ADNS5090 opt_flow_L(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_L,L_MOUSE_PX_PER_MM);
majik 0:21019d94ad33 18 ADNS5090 opt_flow_R(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_R,R_MOUSE_PX_PER_MM);
majik 0:21019d94ad33 19 Reckon reckon(opt_flow_L, opt_flow_R, MOUSE_DISTANCE);
majik 0:21019d94ad33 20
majik 0:21019d94ad33 21 MPU6050 mpu;
majik 0:21019d94ad33 22 MPU6051 mpu2;
majik 0:21019d94ad33 23 DigitalOut imuSwitch(IMU_POWER_PIN);
majik 0:21019d94ad33 24 DigitalOut imu2Switch(IMU2_POWER_PIN);
majik 0:21019d94ad33 25 IMU_DATA imu_data;
majik 0:21019d94ad33 26 IMU_DATA imu2_data;
majik 0:21019d94ad33 27
majik 0:21019d94ad33 28 TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
majik 0:21019d94ad33 29 TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
majik 0:21019d94ad33 30 Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);
majik 0:21019d94ad33 31
majik 0:21019d94ad33 32 nRF24L01P rf(PTD2, PTD3, PTD1, PTD0, PTC17, PTD2); // mosi, miso, sck, csn, ce, irq //CSN has been changed from PTD5 to PTC17
majik 0:21019d94ad33 33
majik 0:21019d94ad33 34
majik 0:21019d94ad33 35 bool send_flag; //indicates when robot should send all data
majik 0:21019d94ad33 36 bool obstacle_flag; //indicates an obstacle - robot should stop
majik 0:21019d94ad33 37 bool fwd_flag;
majik 0:21019d94ad33 38 bool calibrate_flag;
majik 0:21019d94ad33 39 bool calibrate_optFlow_flag;
majik 0:21019d94ad33 40
majik 0:21019d94ad33 41 unsigned long long long_time = 0;
majik 0:21019d94ad33 42 Timer t;
majik 0:21019d94ad33 43
majik 0:21019d94ad33 44 float getTime()
majik 0:21019d94ad33 45 { return (float)(long_time + t.read_us())/1000000 ; }
majik 0:21019d94ad33 46
majik 0:21019d94ad33 47 void initRobot()
majik 0:21019d94ad33 48 {
majik 0:21019d94ad33 49 LED_OFF
majik 0:21019d94ad33 50
majik 0:21019d94ad33 51 MotorA.scale = R_MOTOR_SCALE;
majik 0:21019d94ad33 52 MotorB.scale = L_MOTOR_SCALE;
majik 0:21019d94ad33 53
majik 0:21019d94ad33 54 current_sensor.calibrate();
majik 0:21019d94ad33 55
majik 0:21019d94ad33 56 LED_ON
majik 0:21019d94ad33 57
majik 0:21019d94ad33 58 bt.start(); //this turns on the bluetooth switch
majik 0:21019d94ad33 59 rf.powerUp(); //turn on RF comm
majik 0:21019d94ad33 60 send_flag = 0;
majik 0:21019d94ad33 61 obstacle_flag = 0;
majik 0:21019d94ad33 62
majik 0:21019d94ad33 63 wait_ms(60);
majik 0:21019d94ad33 64 opt_flow_L.setDPI();
majik 0:21019d94ad33 65 opt_flow_R.setDPI();
majik 0:21019d94ad33 66 bt.baud(BT_BAUD_RATE);
majik 0:21019d94ad33 67 motors.flip();
majik 0:21019d94ad33 68
majik 0:21019d94ad33 69
majik 0:21019d94ad33 70 LED_OFF //good to go
majik 0:21019d94ad33 71 t.start(); //start timer
majik 0:21019d94ad33 72 }
majik 0:21019d94ad33 73