Dave Lu / FYDP_Final2

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers robot.cpp Source File

robot.cpp

00001 #include "robot.h"
00002 #include "bot08_cal.h"
00003 
00004 /**Note: Initialize ALL UNUSED ANALOG IN pins as digital out to reduce noise on analog reads*//////
00005 
00006 DigitalOut      led(LED_PIN);
00007 
00008 HC05            bt(TX_BT,RX_BT,EN_BT);
00009 
00010 Servo myservo(PTA12); //PTD0
00011 
00012 AnalogIn irL(IR_L);
00013 AnalogIn irC(IR_C);
00014 AnalogIn irR(IR_R);
00015 DigitalOut irBack(IR_B);
00016 AnalogIn voltage_sensor(VOLTAGESENSOR_PIN);
00017 
00018 ACS712          current_sensor;
00019 ADNS5090        opt_flow_L(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_L,L_MOUSE_PX_PER_MM);
00020 ADNS5090        opt_flow_R(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_R,R_MOUSE_PX_PER_MM);
00021 Reckon          reckon(opt_flow_L, opt_flow_R, MOUSE_DISTANCE);
00022 
00023 MPU6050 mpu;
00024 MPU6051 mpu2;
00025 DigitalOut imuSwitch(IMU_POWER_PIN);
00026 DigitalOut imu2Switch(IMU2_POWER_PIN);
00027 IMU_DATA imu_data;
00028 IMU_DATA imu2_data;
00029 
00030 TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
00031 TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
00032 Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);
00033 
00034 nRF24L01P rf(PTD2, PTD3, PTD1, PTD2, PTC17, PTD2);    // mosi, miso, sck, csn, ce, irq //CSN has been changed from PTD5 to PTC17
00035 
00036 
00037 bool send_flag; //indicates when robot should send all data
00038 bool obstacle_flag; //indicates an obstacle - robot should stop
00039 bool fwd_flag;
00040 bool calibrate_flag;
00041 bool calibrate_optFlow_flag;
00042 
00043 unsigned long long long_time = 0;
00044 Timer t;
00045 //Timer t_m; //Timer for the motor
00046 time_t prev = 0; //previous time for the motor
00047 
00048 float getTime()
00049 {   return (float)(long_time + t.read_us())/1000000 ;   }
00050 
00051 void initRobot()
00052 {   
00053     LED_OFF
00054     
00055     MotorA.scale = R_MOTOR_SCALE;
00056     MotorB.scale = L_MOTOR_SCALE;    
00057     
00058     current_sensor.calibrate();
00059     
00060     LED_ON
00061     
00062     bt.start(); //this turns on the bluetooth switch
00063     rf.powerUp();   //turn on RF comm
00064     send_flag = 0;
00065     obstacle_flag = 0;
00066     
00067     wait_ms(60);
00068     opt_flow_L.setDPI();
00069     opt_flow_R.setDPI();
00070     bt.baud(BT_BAUD_RATE);
00071     motors.flip();
00072     
00073     
00074     LED_OFF //good to go
00075     t.start();      //start timer
00076    //t_m.start();
00077 }
00078