This is for our FYDP project. 2 MPU6050s are used
robot/robot.cpp
- Committer:
- majik
- Date:
- 2015-03-22
- Revision:
- 3:47461d37adfb
- Parent:
- 0:21019d94ad33
File content as of revision 3:47461d37adfb:
#include "robot.h" #include "bot08_cal.h" /**Note: Initialize ALL UNUSED ANALOG IN pins as digital out to reduce noise on analog reads*////// DigitalOut led(LED_PIN); HC05 bt(TX_BT,RX_BT,EN_BT); Servo myservo(PTA12); //PTD0 AnalogIn irL(IR_L); AnalogIn irC(IR_C); AnalogIn irR(IR_R); DigitalOut irBack(IR_B); AnalogIn voltage_sensor(VOLTAGESENSOR_PIN); ACS712 current_sensor; ADNS5090 opt_flow_L(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_L,L_MOUSE_PX_PER_MM); ADNS5090 opt_flow_R(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_R,R_MOUSE_PX_PER_MM); Reckon reckon(opt_flow_L, opt_flow_R, MOUSE_DISTANCE); MPU6050 mpu; MPU6051 mpu2; DigitalOut imuSwitch(IMU_POWER_PIN); DigitalOut imu2Switch(IMU2_POWER_PIN); IMU_DATA imu_data; IMU_DATA imu2_data; TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN); TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN); Motors motors( &MotorA, &MotorB, MOT_STBY_PIN); nRF24L01P rf(PTD2, PTD3, PTD1, PTD2, PTC17, PTD2); // mosi, miso, sck, csn, ce, irq //CSN has been changed from PTD5 to PTC17 bool send_flag; //indicates when robot should send all data bool obstacle_flag; //indicates an obstacle - robot should stop bool fwd_flag; bool calibrate_flag; bool calibrate_optFlow_flag; unsigned long long long_time = 0; Timer t; //Timer t_m; //Timer for the motor time_t prev = 0; //previous time for the motor float getTime() { return (float)(long_time + t.read_us())/1000000 ; } void initRobot() { LED_OFF MotorA.scale = R_MOTOR_SCALE; MotorB.scale = L_MOTOR_SCALE; current_sensor.calibrate(); LED_ON bt.start(); //this turns on the bluetooth switch rf.powerUp(); //turn on RF comm send_flag = 0; obstacle_flag = 0; wait_ms(60); opt_flow_L.setDPI(); opt_flow_R.setDPI(); bt.baud(BT_BAUD_RATE); motors.flip(); LED_OFF //good to go t.start(); //start timer //t_m.start(); }