Skip to content

Commit

Permalink
Merge pull request #19 from KadenKing/Vision
Browse files Browse the repository at this point in the history
Vision
  • Loading branch information
OrangeHoopla authored Apr 15, 2018
2 parents aefe639 + 46a2ba4 commit 7998f17
Show file tree
Hide file tree
Showing 4 changed files with 88 additions and 12 deletions.
70 changes: 70 additions & 0 deletions arduino/IMU
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>


/* This driver reads raw data from the BNO055
Connections
===========
Connect SCL to analog 5
Connect SDA to analog 4
Connect VDD to 3.3V DC
Connect GROUND to common ground
History
=======
2015/MAR/03 - First release (KTOWN)
*/

/* Set the delay between fresh samples. Wait for serial input to define sample rate. */


Adafruit_BNO055 bno = Adafruit_BNO055();


void setup(void){
while(!Serial.available()){
#define BNO055_SAMPLERATE_DELAY_MS (Serial.read())
}
//remove the above serial read after testing and timing diagram is established.
Serial.begin(9600);

/* Initialise the sensor */

if(!bno.begin())
{
/* There was a problem detecting the BNO055 ... check your connections */
Serial.println("ConnectionFail");
while(1);
}


bno.setExtCrystalUse(false);

Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");

}
void loop(){
Serial.println(IMU_data());
}


String IMU_data()
{
String data;
double angle;
// Possible vector values can be:
// - VECTOR_ACCELEROMETER - m/s^2
// - VECTOR_MAGNETOMETER - uT
// - VECTOR_GYROSCOPE - rad/s
// - VECTOR_EULER - degrees
// - VECTOR_LINEARACCEL - m/s^2
// - VECTOR_GRAVITY - m/s^2
imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
angle = atan2(euler.y(),euler.x());
/* Display the floating point data */
data = "angle[ " + String(angle) + "] z[ " + String(euler.z()) + "]";

return data;
}
4 changes: 4 additions & 0 deletions bog/nodes/imu.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,10 @@ def turnCCW(distance,goal):
rospy.loginfo("difference: %s", currentDist)
pub.publish(wheels)
time.sleep(0.05)

while (currentDist > DELAY_CONSTANT_FINE):
direction, currentDist = angleDirection(IMU.angle(),goal)
IMU.rotateToFine(direction,currentDist,goal)

stop()

Expand Down
6 changes: 3 additions & 3 deletions bog/nodes/seeker.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@
from std_msgs.msg import Float32


CASCADE_PATH = "/home/orangehoopla/ieee_ws/src/IEEE2018/bog/nodes/cascade_19.xml" #absolute path to the cascade
DEBUG_DISPLAY = 0 # if true, shows the camera and its interpretations on the picture
CASCADE_PATH = "/home/kaden/Documents/ws/src/IEEE2018/bog/nodes/cascade_19.xml" #absolute path to the cascade
DEBUG_DISPLAY = 1 # if true, shows the camera and its interpretations on the picture


def vision():
Expand All @@ -19,7 +19,7 @@ def vision():

button_cascade = cv2.CascadeClassifier(CASCADE_PATH) # uses the haar cascade in the path to find the button

cap = cv2.VideoCapture(0) # cap is the image that the camera captured
cap = cv2.VideoCapture(1) # cap is the image that the camera captured


while not rospy.is_shutdown():
Expand Down
20 changes: 11 additions & 9 deletions bog/nodes/vision_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@
GOAL_DISTANCE_LOW = 5 # the lower bound of the goal distance from the button
GOAL_DISTANCE_HIGH = 11 # the upper bound of the goal distance from the button
WHEEL_SPEED = 120
TIMEOUT = 0.5


#state variables
current_direction = ""
Expand All @@ -32,13 +34,13 @@ def forward():
wheels.wheel3 = WHEEL_SPEED*0.75
wheels.wheel4 = WHEEL_SPEED*0.75
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)
wheels.wheel1 = 0
wheels.wheel2 = 0
wheels.wheel3 = 0
wheels.wheel4 = 0
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)

def backward():
pub = rospy.Publisher('VISION_Motors', SetWheelSpeeds, queue_size=10)
Expand All @@ -47,13 +49,13 @@ def backward():
wheels.wheel3 = -1*WHEEL_SPEED
wheels.wheel4 = -1*WHEEL_SPEED
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)
wheels.wheel1 = 0
wheels.wheel2 = 0
wheels.wheel3 = 0
wheels.wheel4 = 0
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)


def right():
Expand All @@ -63,13 +65,13 @@ def right():
wheels.wheel3 = 1*WHEEL_SPEED
wheels.wheel4 = -1*WHEEL_SPEED
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)
wheels.wheel1 = 0
wheels.wheel2 = 0
wheels.wheel3 = 0
wheels.wheel4 = 0
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)

def left():
pub = rospy.Publisher('VISION_Motors', SetWheelSpeeds, queue_size=10)
Expand All @@ -78,13 +80,13 @@ def left():
wheels.wheel3 = -1*WHEEL_SPEED
wheels.wheel4 = 1*WHEEL_SPEED
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)
wheels.wheel1 = 0
wheels.wheel2 = 0
wheels.wheel3 = 0
wheels.wheel4 = 0
pub.publish(wheels)
time.sleep(0.5)
time.sleep(TIMEOUT)



Expand All @@ -100,7 +102,7 @@ def move(direction, distance):
right()
elif current_distance > GOAL_DISTANCE_LOW: #move forward
forward()
elif current_distance > GOAL_DISTANCE_HIGH:
elif current_distance < GOAL_DISTANCE_LOW:
backward()


Expand Down

0 comments on commit 7998f17

Please sign in to comment.