Code for our FYDP -only one IMU works right now -RTOS is working
Diff: robot/robot.cpp
- Revision:
- 0:964eb6a2ef00
diff -r 000000000000 -r 964eb6a2ef00 robot/robot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/robot/robot.cpp Wed Mar 18 22:23:48 2015 +0000 @@ -0,0 +1,73 @@ +#include "robot.h" +#include "bot08_cal.h" + +/**Note: Initialize ALL UNUSED ANALOG IN pins as digital out to reduce noise on analog reads*////// + +DigitalOut led(LED_PIN); + +HC05 bt(TX_BT,RX_BT,EN_BT); + +AnalogIn irL(IR_L); +AnalogIn irC(IR_C); +AnalogIn irR(IR_R); +DigitalOut irBack(IR_B); +AnalogIn voltage_sensor(VOLTAGESENSOR_PIN); + +ACS712 current_sensor; +ADNS5090 opt_flow_L(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_L,L_MOUSE_PX_PER_MM); +ADNS5090 opt_flow_R(MOSI_MOUSE,MISO_MOUSE,SCLK_MOUSE,NCS_MOUSE_R,R_MOUSE_PX_PER_MM); +Reckon reckon(opt_flow_L, opt_flow_R, MOUSE_DISTANCE); + +MPU6050 mpu; +MPU6051 mpu2; +DigitalOut imuSwitch(IMU_POWER_PIN); +DigitalOut imu2Switch(IMU2_POWER_PIN); +IMU_DATA imu_data; +IMU_DATA imu2_data; + +TB6612 MotorA(MOT_PWMA_PIN, MOT_AIN1_PIN, MOT_AIN2_PIN); +TB6612 MotorB(MOT_PWMB_PIN, MOT_BIN1_PIN, MOT_BIN2_PIN); +Motors motors( &MotorA, &MotorB, MOT_STBY_PIN); + +nRF24L01P rf(PTD2, PTD3, PTD1, PTD0, PTC17, PTD4); // mosi, miso, sck, csn, ce, irq //CSN has been changed from PTD5 to PTC17 + + +bool send_flag; //indicates when robot should send all data +bool obstacle_flag; //indicates an obstacle - robot should stop +bool fwd_flag; +bool calibrate_flag; +bool calibrate_optFlow_flag; + +unsigned long long long_time = 0; +Timer t; + +float getTime() +{ return (float)(long_time + t.read_us())/1000000 ; } + +void initRobot() +{ + LED_OFF + + MotorA.scale = R_MOTOR_SCALE; + MotorB.scale = L_MOTOR_SCALE; + + current_sensor.calibrate(); + + LED_ON + + bt.start(); //this turns on the bluetooth switch + rf.powerUp(); //turn on RF comm + send_flag = 0; + obstacle_flag = 0; + + wait_ms(60); + opt_flow_L.setDPI(); + opt_flow_R.setDPI(); + bt.baud(BT_BAUD_RATE); + motors.flip(); + + + LED_OFF //good to go + t.start(); //start timer +} +