Embed Size (px)
Citation preview
DobbY
University of Florida
Electrical and Computer Engineering Department
EEL 5666C – Intelligent Machines Design Laboratory
Spring 2013
Rohan Prakash
06655839
Instructors: Teaching Assistants: - Dr. A. Antonio Arroyo - Joshua N Weaver - Dr. Eric M. Schwartz - Ryan Chilton
2 | P a g e
Table of Contents
Abstract……………………………………………………………………………………………………………………………….….03
Executive Summary……………………………………………………………………………………………………………..….04
Introduction……………………………………………………………………………………………………………………….…...05
Integrated System.........................................................................................................................06
Mobile Platform………………………………………………………………………………………………………………….…...07
Actuation………………………………………………………………………………………………………………………………...10
Sensors…………………………………………………………………………………………………………………………………....11
Behavior........................................................................................................................................16
Communication……………………………………………………………………………………………………………………….17
Experimental Layout and Results……………………………………………………………………………………………..19
Conclusion……………………………………………………………………………………………………………………………….20
Documentation………………………………………………………………………….……………………………………….…...21
Appendix.................................................................................................................................…...22
3 | P a g e
Abstract
While working in a lab or a classroom or an office with a group of people one may not
always want to stand up and go to another desk just to pass on a stapler or a component or any
other object. Dobby is here to do that job for you. Dobby will take an object from one place to
another autonomously. You don’t even have to give any command for it to start. Just put the
object on its back and it’ll take it to the desired destination. Once the object is picked up, it will
stop where it is.
4 | P a g e
Executive Summary
Dobby is a four wheeled smart autonomous robot whose main objective is to detect the
presence of an object and carry the same to its destination without colliding into any obstacle.
Dobby uses IR Sensors and Bump Sensors to avoid obstacles and uses an IP camera to detect its
destination. Force Sensitive Resistors are used along with an opamp as a comparator to detect
when an object has been given to Dobby.
On supplying Dobby with an object to transfer, the force sensitive resistors detect a change in
their resistance and hence trigger the processor board. Dobby then starts to move and in
default case keeps moving and avoiding obstacles. In this mode, it looks around for target
destination by keeping track of a particular color. When it encounters the color, Dobby goes
into ‘move towards goal’ mode and starts to move towards the color. While doing so, it is
capable of avoiding obstacles and running into objects.
It basically alternates between ‘move towards goal’ mode and ‘obstacle avoidance’ mode in
order to fulfill its objective. When it reaches its destination and the object is delivered (picked
by user), Dobby stops itself.
The color detection and tracking is done by the use of an IP Camera and the image processing is
done on Laptop using OpenCV 2.4.3. Communication between Camera and Laptop is through a
wireless network by the use of a router. The communication between Laptop and Dobby is
achieved through a Bluetooth Channel.
5 | P a g e
Introduction
Taking breaks in between long working hours while sitting at the same position is always
advised but it may not always be convenient or comfortable. When we have to pass on an object
from a localized place to another, there are two convenient ways. One is to get up and deliver
the object yourself. Though it seems to be the most logical move, it may not always be very
motivating to get up while being involved in some serious work or one may feel too tired to do
the same. The other way is to throw the object and hope that our colleague catches it. Although
it might be the easiest way, it involves risk of not only injury but also damage to property.
This project introduces a third and a very convenient way to solve our problem. Dobby is
the main objective of a project towards Intelligent Machines Design Laboratory. Dobby will carry
any object placed on it from one place to another while safely avoiding any obstacle that may
come in its path. Destination detection is achieved by using IP Camera to detect specified
destination color. The path and obstacle avoidance is done by the use of IR and Bump Sensors.
To detect that an object has been placed and hence to activate it, force sensitive sensors are used
on its back.
When an object is placed on it, it starts moving, avoiding any obstacle that may come in
its way. Simultaneously, it scans the area and as soon as it catches the specified color, it starts
approaching towards it. When it reaches its destination, and the object is picked up, Dobby stops
its motion.
6 | P a g e
Integrated System
Dobby uses Epiphany DIY ATxMEGA128A1U microprocessor board for its functioning. All
the sensors and actuators are attached to this board. Figure1 shows a block diagram of all the
components and related connections. The board receives information from Force Sensitive
Sensors, IR Sensors, Bump Sensors and IP Camera via a Bluetooth link over the laptop which
does Image Processing.
7 | P a g e
Mobile Platform
Basal Wood is used to make the Dobby light weight and of reduced cost. He has two
rectangular platforms, one over the other in a twin bed bunk fashion. The lower platform
contains all the circuit elements including processor board, IR Sensors, circuits for bump sensors
and force sensitive resistor. The upper platform covers the lower one and has an IP Camera
along with the force sensitive resistor where the object to be carried can be kept.
(Figure 2 and 3).
Figure 2: Platforms
8 | P a g e
Figure 3: Lower Platform
Special mounts were made for the IR Sensors. L shaped mounts were built so as to facilitate
movement of sensors in all directions. They could be set facing straight or sideways and their
length could also be altered. (See Figure 4 for details.) The mount could be screwed in three
positions on the lower platform and can also be tilted at various angles. This is to play with
various algorithms and positions and figure out which way obstacle avoidance works best.
9 | P a g e
Figure 4: Sensor Mount
Dobby was first designed using Solidworks. Once all the parts were modeled, it was cut using
the T-Tec machine and then assembled.
Figure 5: Solidworks Layout
10 | P a g e
Actuation
Four strong DC Motors are used for movement by robot. Each motor is a 12V, 200RPM
motor with a torque of approximately 50 oz.-inch. The motors have a gear ratio of 30:1 and a
no load current of about 111mA. The motors are strong and capable of traversing on any kind
of surface. They are attached with a neoprene foam wheel which is sturdy.
Figure 6: Motors
11 | P a g e
Sensors
Dobby uses sensors three kinds of sensors, one each for detection that an object has
been placed over it, obstacle avoidance and color detection to determine destination.
IR SENSORS
Dobby uses 3 IR Sensors for obstacle avoidance. Dobby uses three IR Object Detector Module
(Solutions-Cubed BM007) as shown in Figure 2. These are based on Sharp GP2Y0D810Z0F and
detect an object within 100mm (4”). The module has an in-built analog to digital converter and
hence gives a digital output. The circuit diagram for the same is as shown in Figure 3. The
output goes low when an object is detected and remains high otherwise. It also has an Enable
pin which can be used to enable the IR Sensor when needed. The same is enabled when an
object is placed over it and is detected by the Force Sensitive Sensor. It also has a small LED to
indicate the presence of an object in front of it.
There are three such sensors in front. They are placed in form of a cone and the robot moves as
per the detection of objects in front it. It is capable of backing up in-case the front is blocked or
it’s trapped in a corner.
12 | P a g e
BUMP SENSORS
Two Bump Sensors were added to Dobby to enhance its obstacle avoidance skills and help it to
avoid collision from angles when IR Sensors did not detect an object. Bump Sensors here are
basically push switches which are extended with the use of rubber and a small metal piece to
form a whisker like thing which acts as the bump sensor. The circuit diagram and actual bump
sensor are as shown in Figure 9 and 10.
Figure 9: Circuit Diagram for Switch as a Bump Sensor
Figure 10: Bump Sensor
13 | P a g e
FORCE SENSITIVE SENSORS
Dobby uses force sensitive sensors to sense that an object has been placed over it. This is based
of Force Sensitive Resistor (FSR) as shown in Figure 4. The FSR being used in DobbY is a 1.5”
squared FSR (Pololu Item# 1645) whose resistance changes when a force is applied on it. The
force-resistance relation is inversely proportional. As the amount of force/weight (in grams) is
increased, the resistance of the FSR decreases with the proportional relation as shown in Figure
5.
The resistance changing property is used to enable or disable the IR Sensor Modules. An
op-amp (LM339) is used as a comparator with its non-inverting input connected to the FSR and
the inverting input connected to a potentiometer to regulate the comparing resistance. The
op-amp is used with hysteresis by connecting a feedback resistor from the output. The
sensitivity of the FSR can be regulated by changing the threshold value by altering the resistor
from the potentiometer. The circuit diagram for using FSR and op-amp in a comparator based
circuit is shown in Figure 6. Figure 7 shows a PCB sketch for the same.
14 | P a g e
IP CAMERA
An IP Camera is being used to give eyes to Dobby. It will be used for image processing so as so
determine the destination by detection a specific color. Dobby uses Foscam I8908W (Figure 8)
as its IP camera which will be used for image processing and identification of colors so as to
recognize the destination where the object is to be carried. The Camera will send the image
back to the laptop where the image will be processed and on processing the signal will then be
sent to the robot via Bluetooth Xbee which will guide where and how the robot should proceed.
15 | P a g e
Image processing is done using OpenCV and the color is detected by defining the threshold
value for various colors in HSV. For instance, the HSV for Blue Color is 80-120, 160-256 and 60-
256 respectively for Hue, Saturation and Value. A test image using web-camera was obtained
for blue color within this range as displayed in Figure 16.
16 | P a g e
Behavior
Dobby has two main behaviors to follow. One is obstacle avoidance and another is color
detection. When no color is detected, Dobby roams around searching for colors and avoiding
any obstacles. The processor board takes input from three IR Sensors and two bump sensors in
avoiding obstacles. The sensors are placed in a triangular fashion to determine obstacles from a
wider area and bump sensors act as an extra layer of protection to avoid getting crashed. The
other behavior is color detection and color following.
When dobby detects a color, it starts to move towards it. It basically repeats these two
behaviors. When a color is detected, it moves slightly towards it, then goes forward and does
obstacle avoidance, then again does follows the color and again moves forward avoiding
obstacles and keeps on doing the same until it is directly in front of the desired color.
A short flowchart of behavior of dobby is displayed in Figure 17.
Figure 17:
Behavioral
Flowchart
17 | P a g e
Communication
Dobby needed to communicate with an image processing tool, which detects color, so that it
sends commands and signals to the processor board so as to follow the color. For this purpose,
OpenCV 2.4.3 was used for image processing. OpenCV communicated to Dobby by setting a
Bluetooth channel between the processor board and OpenCV (via Laptop). For this purpose, a
Blue-Bee was used that established a Bluetooth link between the two. Blue Bee has exactly the
same configuration as an X-Bee but works and connects over Bluetooth.
Figure 18:
Blue Bee
A serial COM port communication is set up between processor board and OpenCV. A software
called BlueSoleil Space is used to set up an outgoing COM Port to the Blue-Bee. The IP Camera
sends the video feed to OpenCV, which processes it to identify a particular. Based on the
location of the color, OpenCV sends particular commands to Epiphany DIY via the Bluetooth link
and Epiphany DIY is coded to accept these commands and behave accordingly.
18 | P a g e
Figure 19: Communication Setup
19 | P a g e
Experimental Layout and Results
Dobby went through a lot of changes throughout the design process. In my original design, I
wanted to make a metallic platform with a box like feature for its back but it wasn’t very clean
and was making Dobby unnecessarily heavy which wasn’t of much use to me, so I switched the
platform to wood. I liked the L mounts I made for sensor as it gives me the freedom to align the
sensors in any direction I want and also I am able to adjust how far along I want it to detect
obstacles from. Since Dobby has IR Sensors and not Sonar, there were range issues and
sometimes it would bump into objects simply because the object was not in line of sight of the
IR Sensors. To solve this problem, Dobby was given two Bump Sensors near its wheels which
make it highly efficient to avoid any kind of collision.
Another major issue was with the IP Camera. The camera has an array of IR LEDs which would
switch on incase the ambient light was low. This effected the color detection as it changed the
HSV Values to a great extent. To solve this, there were two plausible solutions. Either to switch
off the IR LEDs every time program was run or to create a program to account for such changes.
I created a program which helped me set and adjust the HSV values in real time. Besides,
solving the LED issue, it helped me to select the destination color in real time and not have
Dobby follow a fixed a color every time.
One problem that remained unresolved was the communication over blue-bee via Bluetooth.
The laptop would not directly connect to the blue bee and communicate without using a
Bluetooth dongle. Even with the dongle, there were certain issues and problems that remained
unanswered. However, I was able to figure out a way to get around with this problem with a
little extra effort. The blue bee has to be disconnected and paired every time a communication
channel is set up between the laptop and processor board.
20 | P a g e
Conclusion
I have always had an interest in Robotics and hence was inclined to take up this course. I had
never worked with a processor board before and it was a nice and easy way to handle a lot of
important aspects of Dobby. One of the many new things I learned in this course was using
SolidWorks as being an Electrical Engineer, I had no experience with it.
Dobby at present, is able to fulfill its main objective but there is much scope of improvement
and advancement. In its current state, Dobby goes to only one particular color. It would be a
more efficient machine if user could select from a list of multiple colors, so that it could track
multiple destinations. Instead of detecting color, it would be a good idea to do facial
recognition. A bigger design might help to carry large objects.
21 | P a g e
Documentation
Epiphany DIY: http://ootbrobotics.com/wp-content/uploads/2012/07/Epiphany-DIY-
Users-Manual-v2-preliminary.pdf
ATxMEGA: http://www.atmel.com/Images/doc8107.pdf
Motor Module: http://www.solutions-
cubed.com/content/Downloads/EasyRollerDocs/ER_DS_9.pdf
Sensor Module: http://www.solutions-
cubed.com/content/Downloads/Breakout%20Modules/DATASHEET_BM007.pdf
BlueBee: http://tutorial.cytron.com.my/2011/08/13/getting-started-with-bluetooth-
bee/
Force Sensitive Resistor:
http://www.pololu.com/file/download/fsr_datasheet.pdf?file_id=0J383
Dobby Website: https://sites.google.com/site/dobbyimdl/home
22 | P a g e
Appendix
SolidWorks Design
Figure 20: Front View
Figure 21: Back View
23 | P a g e
Figure 22: Top View
Figure 23: Angled View
24 | P a g e
Atmel Studio Code (C-Code)
a) Main Code
/* * IMDL Spring 2013 * * DobbY * Author: Rohan */ #include <avr/io.h> #include <util/delay.h> #include <math.h> #include "clock.h" #include "ATtinyServo.h" #include "uart.h" #include "adc.h" #include "motor.h" #include "RTC.h" #include "lcd.h" int main(void) { clockInit(); usartInit(&USARTE1, 9600); motorInit(); LCDInit(); RTC_DelayInit(); sei(); PORTR.DIRSET = 0x02; PORTD.DIRSET = 0x00; // Port D set to Input char input; while(1) {
fprintf(&lcd_str,"\rHi I am Dobby.\nAt Your Service!!"); _delay_ms(1000); void obstacle_avoidance(); uint8_t sample = PORTD.IN; uint8_t FSR_output1 = (sample >> 6) & 1; //LEFT uint8_t FSR_output2 = (sample >> 7) & 1; //RIGHT if((FSR_output1)|(FSR_output2)) { fprintf(&lcd_str,"\rI Got Your Stuff"); PORTR.OUTCLR = 0x02; int r; int p; usartE1_clearBuffer();
25 | P a g e
_delay_ms(100); if(dataInBufE1()) { fprintf(&lcd_str,"\rDestination Found"); input = usartE1_getchar(); if(input=='L') { for(r=0; r<1; r++) { setMotorEffort(1,0,MOTOR_DIR_NEUTRAL); setMotorEffort(2,600,MOTOR_DIR_FORWARD); setMotorEffort(3,600,MOTOR_DIR_FORWARD); setMotorEffort(4,0,MOTOR_DIR_NEUTRAL); } _delay_ms(1000); for(p=0; p<1; p++) { obstacle_avoidance(); } } else if(input == 'R') { for(r=0; r<1; r++) { setMotorEffort(1,600,MOTOR_DIR_FORWARD); setMotorEffort(2,0,MOTOR_DIR_NEUTRAL); setMotorEffort(3,0,MOTOR_DIR_NEUTRAL); setMotorEffort(4,600,MOTOR_DIR_FORWARD); } _delay_ms(1000); for(p=0; p<1; p++) { obstacle_avoidance(); } } } else { fprintf(&lcd_str,"\rSearching..."); obstacle_avoidance(); } } else { PORTR.OUTSET = 0x02; setMotorEffort(1,0,MOTOR_DIR_NEUTRAL); setMotorEffort(2,0,MOTOR_DIR_NEUTRAL); setMotorEffort(3,0,MOTOR_DIR_NEUTRAL); setMotorEffort(4,0,MOTOR_DIR_NEUTRAL); } obstacle_avoidance() { uint8_t sample = PORTD.IN; uint8_t right_sensor = (sample >> 1) & 1;
26 | P a g e
uint8_t center_sensor = (sample >> 3) & 1; uint8_t left_sensor = (sample >> 5) & 1; uint8_t Bump_Left = (sample >> 4) & 1; uint8_t Bump_Right = (sample >> 2) & 1; void bumpleft(); void bumpright(); void back(); if(!Bump_Left) { fprintf(&lcd_str,"\rBumped"); bumpleft(); } else if(!Bump_Right) { fprintf(&lcd_str,"\rBumped"); bumpright(); } //!(right_sensor) signifies obstacle is present if(right_sensor && center_sensor && left_sensor) // No Obstacle { setMotorEffort(1,600,MOTOR_DIR_FORWARD); setMotorEffort(2,600,MOTOR_DIR_FORWARD); setMotorEffort(3,600,MOTOR_DIR_FORWARD); setMotorEffort(4,600,MOTOR_DIR_FORWARD); _delay_ms(100); } else if(right_sensor && center_sensor && !(left_sensor)) //Left Side Obstacle { setMotorEffort(1,650,MOTOR_DIR_FORWARD); setMotorEffort(2,650,MOTOR_DIR_BACKWARD); setMotorEffort(3,650,MOTOR_DIR_BACKWARD); setMotorEffort(4,650,MOTOR_DIR_FORWARD); _delay_ms(100); } else if (right_sensor && !(center_sensor) && !(left_sensor)) //Left+Center Obstacle { setMotorEffort(1,650,MOTOR_DIR_FORWARD); setMotorEffort(2,650,MOTOR_DIR_BACKWARD); setMotorEffort(3,650,MOTOR_DIR_BACKWARD); setMotorEffort(4,650,MOTOR_DIR_FORWARD); _delay_ms(100); } else if (!(right_sensor) && center_sensor && left_sensor) //Right Obstacle { setMotorEffort(1,650,MOTOR_DIR_BACKWARD); setMotorEffort(2,650,MOTOR_DIR_FORWARD); setMotorEffort(3,650,MOTOR_DIR_FORWARD); setMotorEffort(4,650,MOTOR_DIR_BACKWARD); _delay_ms(100); }
27 | P a g e
else if (!(right_sensor) && !(center_sensor) && left_sensor) //Right+Center Obstacle { setMotorEffort(1,650,MOTOR_DIR_BACKWARD); setMotorEffort(2,650,MOTOR_DIR_FORWARD); setMotorEffort(3,650,MOTOR_DIR_FORWARD); setMotorEffort(4,650,MOTOR_DIR_BACKWARD); _delay_ms(100); } else { back(); } } bumpleft() { setMotorEffort(1,600,MOTOR_DIR_BACKWARD); setMotorEffort(2,600,MOTOR_DIR_BACKWARD); setMotorEffort(3,600,MOTOR_DIR_BACKWARD); setMotorEffort(4,600,MOTOR_DIR_BACKWARD); _delay_ms(1000); int k; for (k=0; k<2;k++) { setMotorEffort(1,0,MOTOR_DIR_NEUTRAL); setMotorEffort(2,650,MOTOR_DIR_BACKWARD); setMotorEffort(3,650,MOTOR_DIR_BACKWARD); setMotorEffort(4,0,MOTOR_DIR_NEUTRAL); _delay_ms(1000); } } bumpright() { setMotorEffort(1,600,MOTOR_DIR_BACKWARD); setMotorEffort(2,600,MOTOR_DIR_BACKWARD); setMotorEffort(3,600,MOTOR_DIR_BACKWARD); setMotorEffort(4,600,MOTOR_DIR_BACKWARD); _delay_ms(1000); int j; for (j=0; j<2;j++) { setMotorEffort(1,650,MOTOR_DIR_BACKWARD); setMotorEffort(2,0,MOTOR_DIR_NEUTRAL); setMotorEffort(3,0,MOTOR_DIR_NEUTRAL); setMotorEffort(4,650,MOTOR_DIR_BACKWARD); _delay_ms(1000); } } back() { uint8_t sample = PORTD.IN; uint8_t right_sensor = (sample >> 1) & 1; uint8_t center_sensor = (sample >> 3) & 1; uint8_t left_sensor = (sample >> 5) & 1;
28 | P a g e
setMotorEffort(1,600,MOTOR_DIR_BACKWARD); setMotorEffort(2,600,MOTOR_DIR_BACKWARD); setMotorEffort(3,600,MOTOR_DIR_BACKWARD); setMotorEffort(4,600,MOTOR_DIR_BACKWARD); _delay_ms(1000); int j; for (j=0; j<1;j++) { if(!(left_sensor)) { setMotorEffort(1,650,MOTOR_DIR_FORWARD); setMotorEffort(2,650,MOTOR_DIR_BACKWARD); setMotorEffort(3,650,MOTOR_DIR_BACKWARD); setMotorEffort(4,650,MOTOR_DIR_FORWARD); _delay_ms(1000); } else { setMotorEffort(1,650,MOTOR_DIR_BACKWARD); setMotorEffort(2,650,MOTOR_DIR_FORWARD); setMotorEffort(3,650,MOTOR_DIR_FORWARD); setMotorEffort(4,650,MOTOR_DIR_BACKWARD); _delay_ms(1000); } } }
b) Epiphany DIY – Motors
/* * motorNew.h * * Created: 8/7/2012 10:06:44 PM * Author: Tim */ #include <avr/io.h> #ifndef MOTOR_H_ #define MOTOR_H_ #define MOTOR_PERIOD_gc 1024 //This is the maximum value for PWM. Duty cycle is (value/1024)*100% #define Motor_CLKSEL_DIV_gc TC_CLKSEL_DIV1_gc //setting of a division of 1 prescaler. So in other words there is no prescaler typedef enum motorDirection_enum{ MOTOR_DIR_NEUTRAL = 0, MOTOR_DIR_FORWARD = 1, MOTOR_DIR_BACKWARD = 2 }motorDirection_t; typedef enum motorDirectionHP_enum{ MOTOR_DIR_NEUTRAL_HP = 0, MOTOR_DIR_FORWARD_HP = 5,
29 | P a g e
MOTOR_DIR_BACKWARD_HP = 0xA }motorDirectionHP_t; typedef struct motor0Dir_field{ motorDirection_t direction :2; uint8_t res1 :2; uint8_t res2 :2; uint8_t res3 :2; }m1Dir_t; typedef struct motor1Dir_field{ uint8_t res0 :2; motorDirection_t direction :2; uint8_t res2 :2; uint8_t res3 :2; }m2Dir_t; typedef struct motor2Dir_field{ uint8_t res0 :2; uint8_t res1 :2; motorDirection_t direction :2; uint8_t res3 :2; }m3Dir_t; typedef struct motor3Dir_field{ uint8_t res0 :2; uint8_t res1 :2; uint8_t res2 :2; motorDirection_t direction :2; }m4Dir_t; typedef struct motor1DirHP_field{ motorDirection_t direction :4; uint8_t res1 :4; }m1DirHP_t; typedef struct motor2DirHP_field{ uint8_t res0 :4; motorDirection_t direction :4; }m2DirHP_t; typedef struct motorControl0{ uint16_t *effortLevel; m1Dir_t *ctrlReg; }motorControl1_t; typedef struct motorControl1{ uint16_t *effortLevel; m2Dir_t *ctrlReg; }motorControl2_t; typedef struct motorControl2{ uint16_t *effortLevel; m3Dir_t *ctrlReg; }motorControl3_t; typedef struct motorControl3{ uint16_t *effortLevel;
30 | P a g e
m4Dir_t *ctrlReg; }motorControl4_t; typedef struct motorControl1HP{ uint16_t *effortLevel1; uint16_t *effortLevel2; m1DirHP_t *ctrlReg; }motorControl1HP_t; typedef struct motorControl2HP{ uint16_t *effortLevel1; uint16_t *effortLevel2; m2DirHP_t *ctrlReg; }motorControl2HP_t; void motorInit(); void setMotorEffort(uint8_t motorNum, uint16_t effort, motorDirection_t motorDir); void setMotorEffort_HP(uint8_t hpMotorNum, uint16_t effort, motorDirectionHP_t motorDir); #endif /* MOTORNEW_H_ */ /* * motorNew.c * * Created: 8/7/2012 10:06:20 PM * Author: Tim */ #include "motor.h" extern motorControl1_t motor1 = {&TCF0.CCA,&PORTK.OUT}; extern motorControl2_t motor2 = {&TCF0.CCB,&PORTK.OUT}; extern motorControl3_t motor3 = {&TCF0.CCC,&PORTK.OUT}; extern motorControl4_t motor4 = {&TCF0.CCD,&PORTK.OUT}; extern motorControl1HP_t hpMotor1 = {&TCF0.CCA, &TCF0.CCB, &PORTK.OUT}; extern motorControl2HP_t hpMotor2 = {&TCF0.CCC, &TCF0.CCD, &PORTK.OUT}; void motorInit() { TCF0.PER = MOTOR_PERIOD_gc; TCF0.CTRLA = Motor_CLKSEL_DIV_gc; TCF0.CTRLB = TC0_CCAEN_bm | TC0_CCBEN_bm | TC0_CCCEN_bm | TC0_CCDEN_bm | TC_WGMODE_SS_gc; motor1.ctrlReg->direction = MOTOR_DIR_NEUTRAL; motor2.ctrlReg->direction = MOTOR_DIR_NEUTRAL; motor3.ctrlReg->direction = MOTOR_DIR_NEUTRAL; motor4.ctrlReg->direction = MOTOR_DIR_NEUTRAL; PORTK.DIRSET = 0xFF; PORTF.DIRSET = 0x0F; }
31 | P a g e
void setMotorEffort(uint8_t motorNum, uint16_t effort, motorDirection_t motorDir) { switch(motorNum) { case(1): motor1.ctrlReg->direction = motorDir; *motor1.effortLevel = effort; break; case(2): motor2.ctrlReg->direction = motorDir; *motor2.effortLevel = effort; break; case(3): motor3.ctrlReg->direction = motorDir; *motor3.effortLevel = effort; break; case(4): motor4.ctrlReg->direction = motorDir; *motor4.effortLevel = effort; break; } } void setMotorEffort_HP(uint8_t hpMotorNum, uint16_t effort, motorDirectionHP_t motorDir) { switch(hpMotorNum) { case(1): * hpMotor1.effortLevel1 = effort; * hpMotor1.effortLevel2 = effort; hpMotor1.ctrlReg->direction = motorDir; break; case(2): * hpMotor2.effortLevel1 = effort; * hpMotor2.effortLevel2 = effort; hpMotor2.ctrlReg->direction = motorDir; break; } }
c) Epiphany DIY – UART
/* * uart.h * * Created: 7/7/2011 9:55:37 PM * Author: Tim */ #ifndef UART_H_ #define UART_H_ #include <ctype.h> #include <stdint.h>
32 | P a g e
#include <stdio.h> #include <avr/io.h> #include <avr/interrupt.h> #define bufferSizeRx 64 //1200 #define BAUD1200 3331 #define USART_BAUDCTRLA_1200 BAUD1200 #define USART_BAUDCTRLB_1200 0xC0 | (uint16_t)(0x0FFF & BAUD1200>>8) #define USART_CTRLB_1200 USART_RXEN_bm | USART_TXEN_bm //2400 #define BAUD2400 3329 #define USART_BAUDCTRLA_2400 BAUD2400 #define USART_BAUDCTRLB_2400 0xE0 | (uint16_t)(0x0FFF & BAUD2400>>8) #define USART_CTRLB_2400 USART_RXEN_bm | USART_TXEN_bm //4800 #define BAUD4800 3325 #define USART_BAUDCTRLA_4800 BAUD4800 #define USART_BAUDCTRLB_4800 0xD0 | (uint16_t)(0x0FFF & BAUD4800>>8) #define USART_CTRLB_4800 USART_RXEN_bm | USART_TXEN_bm //9600 #define BAUD9600 3317 #define USART_BAUDCTRLA_9600 BAUD9600 #define USART_BAUDCTRLB_9600 0xC0 | (uint16_t)(0x0FFF & BAUD9600>>8) #define USART_CTRLB_9600 USART_RXEN_bm | USART_TXEN_bm //14400 #define BAUD14400 2206 #define USART_BAUDCTRLA_14400 BAUD14400 #define USART_BAUDCTRLB_14400 0xC0 | (uint16_t)(0x0FFF & BAUD14400>>8) #define USART_CTRLB_14400 USART_RXEN_bm | USART_TXEN_bm //19200 #define BAUD19200 3317 #define USART_BAUDCTRLA_19200 BAUD19200 #define USART_BAUDCTRLB_19200 0xC0 | (uint16_t)(0x0FFF & BAUD19200>>8) #define USART_CTRLB_19200 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //28800 #define BAUD28800 2206 #define USART_BAUDCTRLA_28800 BAUD28800 #define USART_BAUDCTRLB_28800 0xC0 | (uint16_t)(0x0FFF & BAUD28800>>8) #define USART_CTRLB_28800 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //38400 #define BAUD38400 3301 #define USART_BAUDCTRLA_38400 BAUD38400 #define USART_BAUDCTRLB_38400 0xB0 | (uint16_t)(0x0FFF & BAUD38400>>8) #define USART_CTRLB_38400 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //56000 #define BAUD56000 2254
33 | P a g e
#define USART_BAUDCTRLA_56000 BAUD56000 #define USART_BAUDCTRLB_56000 0xB0 | (uint16_t)(0x0FFF & BAUD56000>>8) #define USART_CTRLB_56000 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //57600 #define BAUD57600 2190 #define USART_BAUDCTRLA_57600 BAUD57600 #define USART_BAUDCTRLB_57600 0xB0 | (uint16_t)(0x0FFF & BAUD57600>>8) #define USART_CTRLB_57600 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //115200 #define BAUD115200 2158 #define USART_BAUDCTRLA_115200 BAUD115200 #define USART_BAUDCTRLB_115200 0xA0 | (uint16_t)(0x0FFF & BAUD115200>>8) #define USART_CTRLB_115200 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //125000 #define BAUD125000 1984 #define USART_BAUDCTRLA_125000 BAUD125000 #define USART_BAUDCTRLB_125000 0xA0 | (uint16_t)(0x0FFF & BAUD125000>>8) #define USART_CTRLB_125000 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm //230400 #define BAUD230400 2094 #define USART_BAUDCTRLA_230400 BAUD230400 #define USART_BAUDCTRLB_230400 0x90 | (uint16_t)(0x0FFF & BAUD230400>>8) #define USART_CTRLB_230400 USART_RXEN_bm | USART_TXEN_bm | USART_CLK2X_bm typedef struct{ uint8_t *bufferBegin; uint8_t *bufferEnd; uint8_t *head; uint8_t *tail; uint8_t data[bufferSizeRx]; }bufRx_t; bufRx_t bufRxC0, bufRxC1, bufRxD0, bufRxD1, bufRxE1, bufRxF0, bufRxF1; void usartInit(USART_t *uart, long baud);//inilization routine for uart modules. uart = &USARTxn where x is C-F n is 0-1 void usart_putchar(USART_t *usart, char c); uint8_t usart_getchar(bufRx_t *buffer); void usartStore(bufRx_t *buffer, char c); uint8_t dataInBuf(bufRx_t *buffer); static inline uint8_t dataInBufC0(){ return dataInBuf(&bufRxC0);} static inline uint8_t dataInBufC1(){ return dataInBuf(&bufRxC1);} static inline uint8_t dataInBufD0(){ return dataInBuf(&bufRxD0);} static inline uint8_t dataInBufD1(){ return dataInBuf(&bufRxD1);} static inline uint8_t dataInBufE1(){ return dataInBuf(&bufRxE1);}
34 | P a g e
static inline uint8_t dataInBufF0(){ return dataInBuf(&bufRxF0);} static inline uint8_t dataInBufF1(){ return dataInBuf(&bufRxF1);} static inline void usartC0_putchar(char c){ usart_putchar(&USARTC0,c);} static inline void usartC1_putchar(char c){ usart_putchar(&USARTC1,c);} static inline void usartD0_putchar(char c){ usart_putchar(&USARTD0,c);} static inline void usartD1_putchar(char c){ usart_putchar(&USARTD1,c);} static inline void usartE1_putchar(char c){ usart_putchar(&USARTE1,c);} static inline void usartF0_putchar(char c){ usart_putchar(&USARTF0,c);} static inline void usartF1_putchar(char c){ usart_putchar(&USARTF1,c);} static inline uint8_t usartC0_getchar(){ return usart_getchar(&bufRxC0);} static inline uint8_t usartC1_getchar(){ return usart_getchar(&bufRxC1);} static inline uint8_t usartD0_getchar(){ return usart_getchar(&bufRxD0);} static inline uint8_t usartD1_getchar(){ return usart_getchar(&bufRxD1);} static inline uint8_t usartE1_getchar(){ return usart_getchar(&bufRxE1);} static inline uint8_t usartF0_getchar(){ return usart_getchar(&bufRxF0);} static inline uint8_t usartF1_getchar(){ return usart_getchar(&bufRxF1);} static inline void usartE1_clearBuffer(){bufRxE1.tail = bufRxE1.head;} FILE usartC0_str; FILE usartC1_str; FILE usartD0_str; FILE usartD1_str; FILE usartE1_str; FILE usartF0_str; FILE usartF1_str; #define USB_str usartC0_str #define Xbee_str usartE1_str #define Servo_str usartE0_str #endif /* UART_H_ */
/* * uart.c * * Created: 7/7/2011 9:55:17 PM * Author: Tim */ #include "uart.h" bufRx_t bufRxC0 = {&bufRxC0.data[0],&bufRxC0.data[bufferSizeRx-1],&bufRxC0.data[0],&bufRxC0.data[0],0}, bufRxC1 = {&bufRxC1.data[0],&bufRxC1.data[bufferSizeRx-1],&bufRxC1.data[0],&bufRxC1.data[0],0}, bufRxD0 = {&bufRxD0.data[0],&bufRxD0.data[bufferSizeRx-1],&bufRxD0.data[0],&bufRxD0.data[0],0}, bufRxD1 = {&bufRxD1.data[0],&bufRxD1.data[bufferSizeRx-1],&bufRxD1.data[0],&bufRxD1.data[0],0}, bufRxE1 = {&bufRxE1.data[0],&bufRxE1.data[bufferSizeRx-1],&bufRxE1.data[0],&bufRxE1.data[0],0},
35 | P a g e
bufRxF0 = {&bufRxF0.data[0],&bufRxF0.data[bufferSizeRx-1],&bufRxF0.data[0],&bufRxF0.data[0],0}, bufRxF1 = {&bufRxF1.data[0],&bufRxF1.data[bufferSizeRx-1],&bufRxF1.data[0],&bufRxF1.data[0],0}; //sets up all of the streams FILE usartC0_str = FDEV_SETUP_STREAM(usartC0_putchar, usartC0_getchar, _FDEV_SETUP_RW); FILE usartC1_str = FDEV_SETUP_STREAM(usartC1_putchar, usartC1_getchar, _FDEV_SETUP_RW); FILE usartD0_str = FDEV_SETUP_STREAM(usartD0_putchar, usartD0_getchar, _FDEV_SETUP_RW); FILE usartD1_str = FDEV_SETUP_STREAM(usartD1_putchar, usartD1_getchar, _FDEV_SETUP_RW); FILE usartE1_str = FDEV_SETUP_STREAM(usartE1_putchar, usartE1_getchar, _FDEV_SETUP_RW); FILE usartF0_str = FDEV_SETUP_STREAM(usartF0_putchar, usartF0_getchar, _FDEV_SETUP_RW); FILE usartF1_str = FDEV_SETUP_STREAM(usartF1_putchar, usartF1_getchar, _FDEV_SETUP_RW); //these functions are used for outputing data from the uarts void usart_putchar(USART_t *usart, char c) { while(!(usart->STATUS & USART_DREIF_bm)); usart->DATA = c; } uint8_t usart_getchar(bufRx_t *buffer){ if (buffer->tail == buffer->bufferEnd){ uint8_t temp = *buffer->tail; buffer->tail = buffer->bufferBegin; return temp; } else return *buffer->tail++; } void usartStore(bufRx_t *buffer, char c){ if(buffer->head == buffer->bufferEnd){ *buffer->head = c; buffer->head = buffer->bufferBegin; } else *buffer->head++ = c; } uint8_t dataInBuf(bufRx_t *buffer){ if(buffer->head == buffer->tail) return 0; //no data to be read else return 1; } ISR(USARTC0_RXC_vect){ usartStore(&bufRxC0,USARTC0.DATA); } ISR(USARTC1_RXC_vect){ usartStore(&bufRxC1,USARTC1.DATA); } ISR(USARTD0_RXC_vect){
36 | P a g e
usartStore(&bufRxD0,USARTD0.DATA); } ISR(USARTD1_RXC_vect){ usartStore(&bufRxD1,USARTD1.DATA); } ISR(USARTE1_RXC_vect){ usartStore(&bufRxE1,USARTE1.DATA); } ISR(USARTF0_RXC_vect){ usartStore(&bufRxF0,USARTF0.DATA); } ISR(USARTF1_RXC_vect){ usartStore(&bufRxF1,USARTF1.DATA); } //initialization functions for the uarts void usartInit(USART_t *usart, long baud) { PMIC.CTRL |= PMIC_MEDLVLEX_bm; usart->CTRLA |= USART_RXCINTLVL_MED_gc; switch(baud){ case (1200): usart->CTRLB = USART_CTRLB_1200; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_1200; usart->BAUDCTRLB = USART_BAUDCTRLB_1200; break; case (4800): usart->CTRLB = USART_CTRLB_4800; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_4800; usart->BAUDCTRLB = USART_BAUDCTRLB_4800; break; case (9600): usart->CTRLB = USART_CTRLB_9600; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_9600; usart->BAUDCTRLB = USART_BAUDCTRLB_9600; break; case (14400): usart->CTRLB = USART_CTRLB_14400; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_14400; usart->BAUDCTRLB = USART_BAUDCTRLB_14400; break; case (19200): usart->CTRLB = USART_CTRLB_19200; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_19200; usart->BAUDCTRLB = USART_BAUDCTRLB_19200; break; case (28800):
37 | P a g e
usart->CTRLB = USART_CTRLB_28800; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_28800; usart->BAUDCTRLB = USART_BAUDCTRLB_28800; break; case (38400): usart->CTRLB = USART_CTRLB_38400; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_38400; usart->BAUDCTRLB = USART_BAUDCTRLB_38400; break; case (56000): usart->CTRLB = USART_CTRLB_56000; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_56000; usart->BAUDCTRLB = USART_BAUDCTRLB_56000; break; case (57600): usart->CTRLB = USART_CTRLB_57600; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_57600; usart->BAUDCTRLB = USART_BAUDCTRLB_57600; break; case (115200): usart->CTRLB = USART_CTRLB_115200; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_115200; usart->BAUDCTRLB = USART_BAUDCTRLB_115200; break; case (125000): usart->CTRLB = USART_CTRLB_125000; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_125000; usart->BAUDCTRLB = USART_BAUDCTRLB_125000; break; case (230400): usart->CTRLB = USART_CTRLB_230400; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_230400; usart->BAUDCTRLB = USART_BAUDCTRLB_230400; break; default: //invalid baud rate defaulting to 9600 usart->CTRLB = USART_CTRLB_9600; usart->CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc | USART_CHSIZE_8BIT_gc; usart->BAUDCTRLA = USART_BAUDCTRLA_9600; usart->BAUDCTRLB = USART_BAUDCTRLB_9600; break;
38 | P a g e
} }
d) Epiphany DIY – LCD
/* * lcd.h * * Created: 7/2/2011 2:18:05 AM * Author: Tim */ #ifndef LCD_H_ #define LCD_H_ #include <stdbool.h> #include <avr/io.h> #include <util/delay.h> #include <stdint.h> #include <stdio.h> #define LCD_BUSY_FLAG 7 #define LCD_CLEAR 0x01 #define LCD_HOME 0x02 #define LCD_ENTRYMODE 0x06 #define LCD_DISPLAYCTRL 0x0C #define LCD_SHIFTCTRL 0x14 #define LCD_FUNCSET 0x28 #define LCD_ENABLE 1<<0 #define LCD_RW 1<<1 #define LCD_RS 1<<2 #define LCD_DATA_PIN_msk 0xF0 void LCDInit(void); void LCDSetCursor(uint8_t col, uint8_t row); //private functions void waitWhileLCDBusy(void); void LCDCommand(uint8_t data); void LCDWrite(uint8_t data); void LCDEnableInterrupt(void); void LCDDisableInterrupt(void); void LCDStrobe(uint8_t read); void outNibble(uint8_t nibble,uint8_t RS); int lcd_putchar(char c, FILE *unused);
39 | P a g e
FILE lcd_str; #endif /* LCD_H_ */
/* * lcd.c * * Created: 7/2/2011 2:18:17 AM * Author: Tim */ #include "lcd.h" FILE lcd_str = FDEV_SETUP_STREAM(lcd_putchar, NULL, _FDEV_SETUP_WRITE); void LCDInit(void){ PORTJ.OUTSET = LCD_RS | LCD_RW; PORTJ.DIRSET = LCD_ENABLE | LCD_RS | LCD_RW | LCD_DATA_PIN_msk; PORTJ.PIN4CTRL = 0x10; PORTJ.PIN5CTRL = 0x10; PORTJ.PIN6CTRL = 0x10; PORTJ.PIN7CTRL = 0x10; _delay_ms(10); PORTJ.OUTCLR = LCD_RS | LCD_RW; _delay_ms(50); outNibble(0x30,0); _delay_ms(5); outNibble(0x30,0); _delay_ms(.2); outNibble(0x30,0); outNibble(0x20,0); waitWhileLCDBusy(); LCDCommand(0x28); waitWhileLCDBusy(); LCDCommand(0x0C); waitWhileLCDBusy(); LCDCommand(0x01); waitWhileLCDBusy(); LCDCommand(0x06); } void waitWhileLCDBusy(void){ PORTJ.DIRCLR = LCD_DATA_PIN_msk; PORTJ.OUTCLR = 0xF0; PORTJ.OUTCLR = LCD_RS; PORTJ.OUTSET = LCD_RW; _delay_us(1); LCDStrobe(1); LCDStrobe(0); PORTJ.DIRSET = LCD_DATA_PIN_msk; } void LCDCommand(uint8_t data){ outNibble(data,0);
40 | P a g e
outNibble(data<<4,0); } void LCDWrite(uint8_t data){ waitWhileLCDBusy(); outNibble(data,1); outNibble(data<<4,1); } void LCDSetCursor(uint8_t col, uint8_t row){ } void LCDEnableInterrupt(){ } void LCDDisableInterrupt(){ } void LCDStrobe(uint8_t read){ PORTJ.OUTSET = LCD_ENABLE; _delay_us(3); if(read){ while(PORTJ.IN & 0x80); } PORTJ.OUTCLR = LCD_ENABLE; _delay_us(3); } void outNibble(uint8_t nibble,uint8_t RS) { PORTJ.OUTCLR = LCD_RW; if(RS) PORTJ.OUTSET = LCD_RS; else PORTJ.OUTCLR = LCD_RS; PORTJ.OUTCLR = 0xF0; PORTJ.OUTSET = nibble & 0xF0; LCDStrobe(0); } int lcd_putchar(char c, FILE *unused) { static bool nl_seen,cr_seen; if(cr_seen && c != '\r'){ waitWhileLCDBusy(); LCDCommand(LCD_CLEAR); waitWhileLCDBusy(); LCDCommand(LCD_HOME); waitWhileLCDBusy(); // hd44780_outcmd(HD44780_DDADDR(0)); cr_seen = false; } else if(c == '\r'){ cr_seen = true; } else if (nl_seen && c != '\n') { /* * First character after newline, move cursor to next line. */ waitWhileLCDBusy();
41 | P a g e
LCDCommand(0x80 | 0x40); //hd44780_outcmd(HD44780_DDADDR(0x40)); nl_seen = false; } if (c == '\n') { nl_seen = true; } else { waitWhileLCDBusy(); LCDWrite(c); } return 0; }
Visual Studio Code (OpenCV 2.4.3)
a) Main Code
/* * IMDL Spring 2013 * * DobbY * Author: Rohan */ #include "stdafx.h" #include "opencv2\highgui\highgui.hpp" #include "opencv2\core\core.hpp" #include "opencv2\opencv.hpp" #include <iostream> #include <stdlib.h> #include <stdio.h> #include "SerialPort.h" IplImage* imgTracking; int lastX = -1; int lastY = -1; SerialPort* myport; int lowerH=0; int lowerS=0; int lowerV=0; int upperH=180; int upperS=256; int upperV=256; IplImage* GetThresholdedImage(IplImage* imgHSV) {
42 | P a g e
IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1); cvInRangeS(imgHSV, cvScalar(lowerH,lowerS,lowerV), cvScalar(upperH,upperS,upperV), imgThresh); return imgThresh; } //This function create two windows and 6 trackbars for the "Ball" window void setwindowSettings() { cvNamedWindow("Live"); cvNamedWindow("Detect"); cvNamedWindow("TrackBar"); cvResizeWindow("TrackBar",600,350); cvCreateTrackbar("LowerH", "TrackBar", &lowerH, 180, NULL); cvCreateTrackbar("UpperH", "TrackBar", &upperH, 180, NULL); cvCreateTrackbar("LowerS", "TrackBar", &lowerS, 256, NULL); cvCreateTrackbar("UpperS", "TrackBar", &upperS, 256, NULL); cvCreateTrackbar("LowerV", "TrackBar", &lowerV, 256, NULL); cvCreateTrackbar("UpperV", "TrackBar", &upperV, 256, NULL); } int main() { // create serial port myport = new SerialPort(); // open serial port myport->openPort("COM6", 9600, "8n1"); CvCapture* capture = cvCreateFileCapture("http://192.168.2.3:80/videostream.cgi?user=admin&pwd=123456&resolution=32&rate=0&a=.mjpg"); // For IP cam if(!capture) { printf("Capture failure\n"); return -1; } IplImage* frame=0; frame = cvQueryFrame(capture); if(!frame) return -1; imgTracking=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U, 3); cvZero(imgTracking); //convert the image, 'imgTracking' to black cvNamedWindow("Live"); cvNamedWindow("Detect"); while(true)
43 | P a g e
{ frame = cvQueryFrame(capture); int frame_width = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH); int rightside = (frame_width/2) - 30; int leftside = (frame_width/2) + 30; if(!frame) break; frame=cvCloneImage(frame); cvSmooth(frame, frame, CV_GAUSSIAN,3,3); //smooth the original image IplImage* imgHSV = cvCreateImage(cvGetSize(frame), IPL_DEPTH_8U, 3); cvCvtColor(frame, imgHSV, CV_BGR2HSV); //Change o HSV IplImage* imgThresh = GetThresholdedImage(imgHSV); cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,3,3); //smooth image CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments)); cvMoments(imgThresh1, moments, 1); double moment10 = cvGetSpatialMoment(moments, 1, 0); double moment01 = cvGetSpatialMoment(moments, 0, 1); double area = cvGetCentralMoment(moments, 0, 0); if(area>1000) { // calculate the position int posX = moment10/area; int posY = moment01/area; if(posX>leftside) { myport->send("L", 1); printf("L \n"); } else if(posX<rightside) { myport->send("R", 1); printf("R \n"); } else { myport->send("F", 1); printf("F \n"); } } else { myport->send("F", 1); printf("F \n"); }
44 | P a g e
free(moments); cvAdd(frame, imgTracking, frame); cvShowImage("Live", imgThresh); cvShowImage("Detect", frame); //Clean up used images cvReleaseImage(&imgHSV); cvReleaseImage(&imgThresh); cvReleaseImage(&frame); //Wait 10mS int c = cvWaitKey(10); //If 'ESC' is pressed, break the loop if((char)c==27 ) break; } cvDestroyAllWindows(); cvReleaseImage(&imgTracking); cvReleaseCapture(&capture); return 0; }
b) Serial
// File: SerialPort.cpp // Version: 0.1 // Written by: Ryan Chilton, James // Date: 03/20/2009 // last Revision: 03/20/2009 // Description: Cross Platform Serial Port Communications Class // Change Log: 3/18: added the setVTime() function in the windows implementation. // implemented the portCtl() function in the windows implementation. -ryan // 4/2: added the getBytesAvail() function to return the number // of bytes available in the input buffer. - ryan // 4/30: added timedrecv() function for windows (could be improved) // 4/30: changed all functions to work with unsigned chars instead of chars // 5/6: Added functions to take different char types in recv and send. (unsigned char* vs char*) // 5/7: Nicholas: Added timedRecv(char*, int, double) method. // linux includes #if defined(__linux) || defined(linux) || defined(__linux__) #include <sgtty.h> #include <sys/file.h> #include <termios.h> #include <unistd.h> #include <linux/serial.h> #include <stdlib.h> #include <time.h>
45 | P a g e
#include <fcntl.h> #elif defined(_WIN32) // windows includes #include <windows.h> #include <commctrl.h> #include <strsafe.h> #endif // mutual includes #include <iostream> #include <stdio.h> #include "SerialPort.h" #include "TimeLib.h" using namespace std; ///////////////////////////////////////////////////////////////////////// ///// LINUX IMPLEMENTATION //////// ///////////////////////////////////////////////////////////////////////// //#if defined(__linux) || defined(linux) || defined(__linux__) SerialPort::SerialPort() { portHandle = INVALID_HANDLE_VALUE; isOpenFlag = false; } SerialPort::~SerialPort() { } int SerialPort::closePort() { int retVal = 1; if (portHandle != INVALID_HANDLE_VALUE) { #if defined(__linux) || defined(linux) || defined(__linux__) retVal = close(portHandle); #elif defined (_WIN32) CloseHandle(portHandle); #endif } portHandle = INVALID_HANDLE_VALUE; isOpenFlag = false; return retVal;
46 | P a g e
} void SerialPort::openPort(int portNum, int baud, const char* control) { char portName[32]; #if defined(__linux) || defined(linux) || defined(__linux__) /* all ports greater than 100 are USB ports */ if (portNum >= USB_PORT_START) { sprintf(portName, "/dev/ttyUSB%d", portNum - USB_PORT_START); } else { sprintf(portName, "/dev/ttyS%d", portNum - 1); } #elif defined(_WIN32) // the '\\\\.\\' part directly addresses the COM port in windows instead of // using the system defined 'COM1' through 'COM9' sprintf(portName, "\\\\.\\COM%d", portNum); #endif openPort(portName, baud, control); } /** * Open the port */ void SerialPort::openPort(string name, int baud, const char* control) { portName = name; /// Save the Port Name #if defined(__linux) || defined(linux) || defined(__linux__) int bit, stop; char par; tcflag_t bitFlag[] = {CS5, CS6, CS7, CS8}; tcflag_t PAR_CTRL = IGNPAR; tcflag_t BITS; tcflag_t PARITY = 0x00; tcflag_t STOP_BIT = 0x00; tcflag_t SPEED = B19200; struct termios t; struct serial_struct serialInfo; if( (portHandle = open(portName.c_str(), O_RDWR )) == -1 ) { throw SerialPortOpenPortException(); } // Strip the control parameters from the control string sscanf(control, "%1d%1c%1d", &bit, &par, &stop );
47 | P a g e
// Read in the current port attributes if( tcgetattr(portHandle, &t) == -1 ) { throw SerialPortOpenPortException(); } //Get the structure of the serial port if (ioctl(portHandle, TIOCGSERIAL, &serialInfo) != 0) { throw SerialPortOpenPortException(); } // Set the # bits, parity and stop bits if( (bit < 5) || (bit > 8) ) { throw SerialPortOpenPortException(); } BITS = bitFlag[bit-5]; if( par == 'E' || par == 'e' ) { PARITY = PARENB; // Even parity PAR_CTRL = INPCK; // Enable parity check } else if( par == 'O' || par == 'o' ) { PARITY = PARENB | PARODD; // Odd parity PAR_CTRL = INPCK; // Enable parity check } if( stop == 2 ) { STOP_BIT = CSTOPB; // Two stop bits } // Set the baud rate if ( baud == 1200 ) { SPEED = B1200; } else if ( baud == 2400 ) { SPEED = B2400; } else if ( baud == 4800 ) { SPEED = B4800; } else if ( baud == 9600 ) { SPEED = B9600; } else if ( baud == 19200 ) { SPEED = B19200; } else if ( baud == 38400 )
48 | P a g e
{ SPEED = B38400; } else if ( baud == 57600 ) { SPEED = B57600 ; } else if ( baud == 115200) { SPEED = B115200; } else if ( baud == 500000 ) { serialInfo.flags = ASYNC_SPD_CUST; serialInfo.custom_divisor = serialInfo.baud_base / 500000 ; SPEED = B38400; } if (ioctl(portHandle, TIOCSSERIAL, &serialInfo) != 0) { throw SerialPortOpenPortException(); } // Input Modes t.c_iflag &= ~( BRKINT | // Ignore break ISTRIP | // Don't mask INLCR | // Leave NL ICRNL | // Leave CR IGNCR | // " " IXON | // No flow control IXOFF // No flow control ); t.c_iflag |= ( IGNBRK | // Ignore break PAR_CTRL // Parity control ); // Output Modes t.c_oflag &= ~( OPOST ); // No output flags // Control Modes t.c_cflag &= ~( CSIZE | // bits-per-byte CSTOPB | // Two stop bits HUPCL | // Hang up PARENB // Parity ); t.c_cflag |= ( CLOCAL | // Ignore modem status lines CREAD | // Enable receiver STOP_BIT| // # stop bits BITS | // bits-per-byte PARITY // Parity check ); // Local Modes t.c_lflag &= ~( ECHO | // No echo of any kind
49 | P a g e
ECHOE | ECHOK | ECHONL | ICANON | // No canonical input ISIG | // No signal generation NOFLSH | // Enable flushing TOSTOP // No job control ); // Control Characters // When VMIN and VTIME are zero the read() function will // return as much data as possible without waiting t.c_cc[VMIN] = 1; t.c_cc[VTIME] = 0; // Set the baud rate if ( cfsetispeed( &t, SPEED ) == -1 ) { throw SerialPortOpenPortException(); } if ( cfsetospeed( &t, SPEED ) == -1 ) { throw SerialPortOpenPortException(); } if ( tcsetattr(portHandle, TCSANOW, &t) == -1 ) { throw SerialPortOpenPortException(); } #elif defined (_WIN32) DCB dcbSerialParams = {0}; COMMTIMEOUTS cto = {0}; int bit; int stop; char par; portHandle = CreateFileA(portName.c_str(), GENERIC_WRITE | GENERIC_READ, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0); if(portHandle == INVALID_HANDLE_VALUE) { cout << "Error: " << (int)GetLastError() << endl; return; } //config DCB structure //DCB : Device Control Block dcbSerialParams.DCBlength = sizeof(dcbSerialParams); if(!GetCommState(portHandle, &dcbSerialParams)) { return; }
50 | P a g e
// get control parameters from input string sscanf(control, "%d%c%d", &bit, &par, &stop); // set byte size if (bit >= 5 && bit <=8) { dcbSerialParams.ByteSize = bit; } else { printf("Error: invalid byte size requested. Using 8 bits as default."); dcbSerialParams.ByteSize = 8; } // set parity mode if (par == 'n' || par == 'N') { dcbSerialParams.Parity = NOPARITY; dcbSerialParams.fParity = 0; } else if (par == 'e' || par == 'E') { dcbSerialParams.Parity = EVENPARITY; // odd parity check dcbSerialParams.fParity = 1; } else if (par == 'o' || par == 'O') { dcbSerialParams.Parity = ODDPARITY; dcbSerialParams.fParity = 1; } if (stop == 2) { dcbSerialParams.StopBits = TWOSTOPBITS; } else { dcbSerialParams.StopBits = ONESTOPBIT; } if (baud == 1200) { dcbSerialParams.BaudRate = CBR_1200; } else if (baud == 2400) { dcbSerialParams.BaudRate = CBR_2400; } else if (baud == 4800) { dcbSerialParams.BaudRate = CBR_4800; } else if (baud == 9600) { dcbSerialParams.BaudRate = CBR_9600; } else if (baud == 19200)
51 | P a g e
{ dcbSerialParams.BaudRate = CBR_19200; } else if (baud == 38400) { dcbSerialParams.BaudRate = CBR_38400; } else if(baud == 57600) { dcbSerialParams.BaudRate = CBR_57600; } else if (baud == 115200) { dcbSerialParams.BaudRate = CBR_115200; } else if (baud == 128000) { dcbSerialParams.BaudRate = CBR_128000; } else if (baud == 256000) { dcbSerialParams.BaudRate = CBR_256000; } else if (baud == 500000) { dcbSerialParams.BaudRate = 500000; } if(!SetCommState(portHandle, &dcbSerialParams)) { return; } cto.ReadIntervalTimeout = 300; cto.ReadTotalTimeoutConstant = 3500; cto.ReadTotalTimeoutMultiplier = 0; cto.WriteTotalTimeoutConstant = 0; cto.WriteTotalTimeoutMultiplier = 0; if(!SetCommTimeouts(portHandle, &cto)) { return; } #endif isOpenFlag = true; } int SerialPort::portCtl(int request) { int retVal = -1; #if defined(__linux) || defined(linux) || defined(__linux__)
52 | P a g e
if(request == FLUSH_IN_BUF) { retVal = tcflush(portHandle, TCIFLUSH); } else if(request == FLUSH_OUT_BUF) { retVal = tcflush(portHandle, TCOFLUSH); } else if(request == FLUSH_BOTH_BUF) { retVal = tcflush(portHandle, TCIOFLUSH); } #elif defined (_WIN32) if(request == FLUSH_IN_BUF) { // flush the input buffer if (PurgeComm(portHandle, PURGE_RXCLEAR) != 0) { retVal = 1; } } else if(request == FLUSH_OUT_BUF) { // flush the output buffer if (PurgeComm(portHandle, PURGE_TXCLEAR) != 0) { retVal = 1; } } else if(request == FLUSH_BOTH_BUF) { if ((PurgeComm(portHandle, PURGE_RXCLEAR) != 0) && (PurgeComm(portHandle, PURGE_TXCLEAR) != 0)) { retVal = 1; } #endif return retVal; } int SerialPort::setVMin(int vmin) { #if defined(__linux) || defined(linux) || defined(__linux__) struct termios t; // Read in the current port attributes if( tcgetattr(portHandle, &t) == -1 ) { return -1; } t.c_cc[VMIN] = vmin;
53 | P a g e
if( tcsetattr(portHandle, TCSANOW, &t) == -1 ) { return -1; } #elif defined (_WIN32) return -1; #endif return 0; } /** * @throw */ int SerialPort::setVTime(int vtime) { #if defined(__linux) || defined(linux) || defined(__linux__) struct termios t; // Read in the current port attributes if( tcgetattr(portHandle, &t) == -1 ) { return -1; } t.c_cc[VTIME] = vtime; if( tcsetattr(portHandle, TCSANOW, &t) == -1 ) { return -1; } #elif defined (_WIN32) COMMTIMEOUTS cto; // set the amount of time allowed before returning to the calling function // regardless of whether or not all the characters requested have arrived yet. cto.ReadTotalTimeoutConstant = vtime; cto.ReadIntervalTimeout = 0; cto.ReadTotalTimeoutMultiplier = 0; cto.WriteTotalTimeoutConstant = 0; cto.WriteTotalTimeoutMultiplier = 0; if(!SetCommTimeouts(portHandle,&cto)) { return -1; // return 0 if failed } #endif return 0;
54 | P a g e
} int SerialPort::recv(char* buffer, int length) { return recv((unsigned char*)buffer, length); //return recv((char*)buffer, length); } int SerialPort::recv(unsigned char *buffer, int length) { int bytesRecv = -1; #if defined(__linux) || defined(linux) || defined(__linux__) bytesRecv = read(portHandle, buffer, length); #elif defined (_WIN32) DWORD numBytesReceived = 0; ReadFile(portHandle, buffer + numBytesReceived, length, &numBytesReceived, NULL); bytesRecv = static_cast<int>(numBytesReceived); /// return the size of the data sent #endif return bytesRecv; } int SerialPort::timedRecv(char* buffer, int length, double msec) { return timedRecv((unsigned char*)buffer, length, msec); } int SerialPort::timedRecv(unsigned char* buffer, int length, double msec) { int bytesRecv = -1; int count = 0; int maxTries = msec/5; // try this many times if (maxTries == 0) { maxTries = 1; } while (getBytesAvail() < length && count < maxTries) { ojSleepMsec(5); count++; }
55 | P a g e
if (count < maxTries && getBytesAvail() >= length) // got enough chars in the buffer now { // bytesRecv = read(portHandle, buffer, length); bytesRecv = recv(buffer, length); } //#endif return bytesRecv; } int SerialPort::send(const unsigned char* buffer, int length) { return send((unsigned char*)buffer, length); } int SerialPort::send(const char* buffer, int length) { return send((unsigned char*)buffer, length); } int SerialPort::send(char* buffer, int length) { return send((unsigned char*)buffer, length); } int SerialPort::send(unsigned char* buffer, int length) { int bytesSent = -1; #if defined(__linux) || defined(linux) || defined(__linux__) bytesSent = write(portHandle, buffer, length); #elif defined (_WIN32) int temp; DWORD numBytesSent; temp = WriteFile(portHandle, buffer, length, &numBytesSent, NULL); //return numBytesSent;// return the size of the data sent bytesSent = static_cast<int>(numBytesSent); #endif return bytesSent; } int SerialPort::getBytesAvail() {
56 | P a g e
int bytesAvail = 0; #if defined(__linux) || defined(linux) || defined(__linux__) ioctl(portHandle, FIONREAD, &bytesAvail); #elif defined (_WIN32) struct _COMSTAT status; unsigned long etat; ClearCommError(portHandle, &etat, &status); bytesAvail = status.cbInQue; #endif return bytesAvail; } bool SerialPort::isOpen() { return isOpenFlag; }
c) Time
/***************************************************************************** * Copyright (c) 2008, University of Florida * All rights reserved. * * This file is part of OpenJAUS. OpenJAUS is distributed under the BSD * license. See the LICENSE file for details. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of the University of Florida nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
57 | P a g e
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ****************************************************************************/ // File Name: timeLib.c // // Written by: Tom Galluzzo (galluzzo AT gmail DOT com) // // Version: 3.3.0a // // Date: 08/07/08 // // Description: CIMAR timing library header file #include "TimeLib.h" #ifdef WIN32 #include <time.h> #include <windows.h> double ojGetTimeSec(void) { // TODO: the following code could have a 1 second inaccuracy if a second transition occurs after the GetSystemTime call SYSTEMTIME st; GetSystemTime(&st); return time(NULL) + st.wMilliseconds / 1000.0; } void ojSleepMsec(int msec) { Sleep(msec); } #else #include <sys/time.h> #include <time.h> #include <unistd.h> double getTimeSeconds(void) { static struct timeval time; gettimeofday(&time, NULL); return (double)time.tv_sec + (double)time.tv_usec/1.0e6; } double ojGetTimeSec(void) { static struct timeval time; gettimeofday(&time, NULL); return (double)time.tv_sec + (double)time.tv_usec/1.0e6; } void ojSleepMsec(int msec) { usleep(msec * 1000); }
#endif
