Mark Vandermeulen / FYDP_Final2

This is for our FYDP project. 2 MPU6050s are used

Dependencies:   Servo mbed

robot/robot.cpp@4:05484073a641, 2015-03-22 (annotated)

Committer:
majik
Date:
Sun Mar 22 06:34:30 2015 +0000
Revision:
4:05484073a641
Parent:
3:47461d37adfb 
BOTH IMUs WORK NOW. Put them in separate threads. Servo is included.

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 3:47461d37adfb 10 Servo myservo(PTA12); //PTD0
majik 3:47461d37adfb 11
majik 0:21019d94ad33 12 AnalogIn irL(IR_L);
majik 0:21019d94ad33 13 AnalogIn irC(IR_C);
majik 0:21019d94ad33 14 AnalogIn irR(IR_R);
majik 0:21019d94ad33 15 DigitalOut irBack(IR_B);
majik 0:21019d94ad33 16 AnalogIn voltage_sensor(VOLTAGESENSOR_PIN);
majik 0:21019d94ad33 17
majik 0:21019d94ad33 18 ACS712 current_sensor;
majik 0:21019d94ad33 19 ADNS5090 opt_flow_L(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_L,L_MOUSE_PX_PER_MM);
majik 0:21019d94ad33 20 ADNS5090 opt_flow_R(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_R,R_MOUSE_PX_PER_MM);
majik 0:21019d94ad33 21 Reckon reckon(opt_flow_L, opt_flow_R, MOUSE_DISTANCE);
majik 0:21019d94ad33 22
majik 0:21019d94ad33 23 MPU6050 mpu;
majik 0:21019d94ad33 24 MPU6051 mpu2;
majik 0:21019d94ad33 25 DigitalOut imuSwitch(IMU_POWER_PIN);
majik 0:21019d94ad33 26 DigitalOut imu2Switch(IMU2_POWER_PIN);
majik 0:21019d94ad33 27 IMU_DATA imu_data;
majik 0:21019d94ad33 28 IMU_DATA imu2_data;
majik 0:21019d94ad33 29
majik 0:21019d94ad33 30 TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN);
majik 0:21019d94ad33 31 TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN);
majik 0:21019d94ad33 32 Motors motors( &MotorA, &MotorB, MOT_STBY_PIN);
majik 0:21019d94ad33 33
majik 3:47461d37adfb 34 nRF24L01P rf(PTD2, PTD3, PTD1, PTD2, PTC17, PTD2); // mosi, miso, sck, csn, ce, irq //CSN has been changed from PTD5 to PTC17
majik 0:21019d94ad33 35
majik 0:21019d94ad33 36
majik 0:21019d94ad33 37 bool send_flag; //indicates when robot should send all data
majik 0:21019d94ad33 38 bool obstacle_flag; //indicates an obstacle - robot should stop
majik 0:21019d94ad33 39 bool fwd_flag;
majik 0:21019d94ad33 40 bool calibrate_flag;
majik 0:21019d94ad33 41 bool calibrate_optFlow_flag;
majik 0:21019d94ad33 42
majik 0:21019d94ad33 43 unsigned long long long_time = 0;
majik 0:21019d94ad33 44 Timer t;
majik 3:47461d37adfb 45 //Timer t_m; //Timer for the motor
majik 3:47461d37adfb 46 time_t prev = 0; //previous time for the motor
majik 0:21019d94ad33 47
majik 0:21019d94ad33 48 float getTime()
majik 0:21019d94ad33 49 { return (float)(long_time + t.read_us())/1000000 ; }
majik 0:21019d94ad33 50
majik 0:21019d94ad33 51 void initRobot()
majik 0:21019d94ad33 52 {
majik 0:21019d94ad33 53 LED_OFF
majik 0:21019d94ad33 54
majik 0:21019d94ad33 55 MotorA.scale = R_MOTOR_SCALE;
majik 0:21019d94ad33 56 MotorB.scale = L_MOTOR_SCALE;
majik 0:21019d94ad33 57
majik 0:21019d94ad33 58 current_sensor.calibrate();
majik 0:21019d94ad33 59
majik 0:21019d94ad33 60 LED_ON
majik 0:21019d94ad33 61
majik 0:21019d94ad33 62 bt.start(); //this turns on the bluetooth switch
majik 0:21019d94ad33 63 rf.powerUp(); //turn on RF comm
majik 0:21019d94ad33 64 send_flag = 0;
majik 0:21019d94ad33 65 obstacle_flag = 0;
majik 0:21019d94ad33 66
majik 0:21019d94ad33 67 wait_ms(60);
majik 0:21019d94ad33 68 opt_flow_L.setDPI();
majik 0:21019d94ad33 69 opt_flow_R.setDPI();
majik 0:21019d94ad33 70 bt.baud(BT_BAUD_RATE);
majik 0:21019d94ad33 71 motors.flip();
majik 0:21019d94ad33 72
majik 0:21019d94ad33 73
majik 0:21019d94ad33 74 LED_OFF //good to go
majik 0:21019d94ad33 75 t.start(); //start timer
majik 3:47461d37adfb 76 //t_m.start();
majik 0:21019d94ad33 77 }
majik 0:21019d94ad33 78