Mark Vandermeulen / FYDP_Final

Code for our FYDP -only one IMU works right now -RTOS is working

Dependencies:   mbed

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?

UserRevisionLine numberNew 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