This is for our FYDP project. 2 MPU6050s are used
robot/robot.cpp@3:47461d37adfb, 2015-03-22 (annotated)
- Committer:
- majik
- Date:
- Sun Mar 22 00:43:42 2015 +0000
- Revision:
- 3:47461d37adfb
- Parent:
- 0:21019d94ad33
1 IMU, Servo added; -PTD0 is used by something mysterious, so I had to move the servo control line to PTA12
Who changed what in which revision?
User | Revision | Line number | New 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 |