diff --git a/include/sensors/currently_unused/data_source_us.h b/include/sensors/currently_unused/data_source_us.h deleted file mode 100644 index 4818495..0000000 --- a/include/sensors/currently_unused/data_source_us.h +++ /dev/null @@ -1,21 +0,0 @@ -#pragma once -#include "behaviour_control/data_source.h" - -#define US_WAIT_TIME 70 - -class DataSourceUS : public DataSource{ - - public: - DataSourceUS(TwoWire* i2c_, int addr); - void test() override; - void readSensor() override; - - void usMode(); - void usTrigger(); - void usReceive(); - - int reading; - long us_t0; - long us_t1; - bool us_flag; -}; \ No newline at end of file diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index 0c0f255..fc19a93 100644 --- a/include/sensors/sensors.h +++ b/include/sensors/sensors.h @@ -21,8 +21,6 @@ #include "sensors/data_source_bno055.h" #include "sensors/data_source_camera_conicmirror.h" #include "sensors/data_source_camera_vshapedmirror.h" -#include "sensors/currently_unused/data_source_us.h" - void initSensors(); diff --git a/src/sensors/currently_unused/data_source_us.cpp b/src/sensors/currently_unused/data_source_us.cpp deleted file mode 100644 index 0619dd2..0000000 --- a/src/sensors/currently_unused/data_source_us.cpp +++ /dev/null @@ -1,118 +0,0 @@ -#include "behaviour_control/status_vector.h" -#include "sensors/currently_unused/data_source_us.h" -#include "vars.h" - - -DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr){ - us_flag = false; - value = 0; -} - -void DataSourceUS::test(){ - - // test from srf02_example - // step 1: instruct sensor to read echoes - // transmit to device #112 (0x70) - Wire1.beginTransmission(i2CAddr); - // sets register pointer to the command register (0x00) - Wire1.write(byte(0x00)); - // command sensor to measure in "centimeters" (0x51). 0x50 inches and 0x52 - // microseconds - Wire1.write(byte(0x51)); - // stop transmitting - Wire1.endTransmission(); - // step 2: wait for readings to happen - // datasheet suggests at least 65 milliseconds - delay(70); - - // step 3: instruct sensor to return a particular echo reading - // transmit to device #112 - Wire1.beginTransmission(i2CAddr); - // sets register pointer to echo #1 register (0x02) - Wire1.write(byte(0x02)); - Wire1.endTransmission(); - - // step 4: request reading from sensor - // request 2 bytes from slave device #112 - Wire1.requestFrom(i2CAddr, 2); - - // step 5: receive reading from sensor - if (2 <= Wire1.available()) { - // if two bytes were received - // receive high byte (overwrites previous reading) - reading = Wire1.read(); - // shift high byte to be high 8 bits - reading = reading << 8; - // receive low byte as lower 8 bit - reading |= Wire1.read(); - value = reading; - } - - DEBUG.println(value); -} - -void DataSourceUS::readSensor(){ - if (us_flag == false) { - usTrigger(); - us_flag = true; - us_t0 = millis(); - } else { - us_t1 = millis(); - if ((us_t1 - us_t0) > US_WAIT_TIME) { - usReceive(); - us_flag = false; - } - } -} - -void DataSourceUS::usTrigger() { - // step 1: instruct sensor to read echoes - // transmit to device #112 (0x70) - i2c->beginTransmission(i2CAddr); - // sets register pointer to the command register (0x00) - i2c->write(byte(0x00)); - // command sensor to measure in "centimeters" (0x51). 0x50 inches and 0x52 - // microseconds - i2c->write(byte(0x51)); - i2c->endTransmission(); -} - -void DataSourceUS::usReceive() { - // transmit to device #112s - i2c->beginTransmission(i2CAddr); - // sets register pointer to echo 1 register(0x02) - i2c->write(byte(0x02)); - i2c->endTransmission(); - - // step 4: request reading from sensor - // request 2 bytes from slave device #112 - i2c->requestFrom(i2CAddr, 2); - - // step 5: receive reading from sensor - // receive high byte (overwrites previous reading) - reading = i2c->read(); - // shift high byte to be high 8 bits - reading = reading << 8; - // receive low byte as lower 8 bit - reading |= i2c->read(); - value = reading; - - switch(i2CAddr){ - case 112: - CURRENT_INPUT_WRITE.USfr = value; - CURRENT_DATA_WRITE.USfr = value; - break; - case 113: - CURRENT_INPUT_WRITE.USdx = value; - CURRENT_DATA_WRITE.USdx = value; - break; - case 114: - CURRENT_INPUT_WRITE.USrr = value; - CURRENT_DATA_WRITE.USrr = value; - break; - case 115: - CURRENT_INPUT_WRITE.USsx = value; - CURRENT_DATA_WRITE.USsx = value; - break; - } -} \ No newline at end of file diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index 13afdc4..3298bbf 100644 --- a/src/sensors/sensors.cpp +++ b/src/sensors/sensors.cpp @@ -8,9 +8,7 @@ void initSensors(){ pinMode(LED_R, OUTPUT); pinMode(LED_Y, OUTPUT); pinMode(LED_G, OUTPUT); - - dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), - new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) }; + drive = new DriveController(new Motor(11, 12, 4, 45),new Motor(24, 25, 5, 135), new Motor(26, 27, 2, 225), new Motor(28, 29, 3, 315)); //drive = new DriveController(new Motor(12, 11, 2, 45),new Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315)); compass = new DataSourceBNO055(); @@ -18,7 +16,6 @@ void initSensors(){ //ball = new DataSourceBall(&Serial4, 57600); camera = new DataSourceCameraConic(&Serial3, 19200); //camera = new DataSourceCameraConic(&Serial2, 19200); - usCtrl = new DataSourceCtrl(dUs); bt = new DataSourceBT(&Serial1, 115200); //bt = new DataSourceBT(&Serial3, 115200); }