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