// 해당 소스코드는 외부데이터(미세먼지 / 온도 / 습도 / 조도) 측정하고, 내부 데이터와 비교하여
// 외부환기에 대한 부분을 담당하는 부분이다.

#include “SoftwareSerial.h”
#include <Adafruit_NeoPixel.h>
#include “DHT.h”
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

// 블루통신 관련해서 필요한 변수 정리
const int maxIndex = 10;
byte blockData[maxIndex]; //block 값 저장
int arrIndex = 0; // 배열 arrIndex

byte refined_humidity = 0;
byte refined_temperature = 0;
byte refined_value = 0;
byte refined_dust_data = 0;

byte refined_humidity2 = 0;
byte refined_temperature2 = 0;
byte refined_value2 = 0;
byte refined_dust_data2 = 0;

/*미세먼지센서 변수들*/
#define measurePin A1 //Connect dust sensor to Arduino A0 pin
#define ledPower 6 //Connect 3 led driver pins of dust sensor to Arduino D4
#define SampleTime 30 //먼지센서 30회 샘플링

int samplingTime = 280;
int deltaTime = 40;
int sleepTime = 9680;
float voMeasured = 0;
float calcVoltage = 0;
float dustDensity = 0;

//조도센서——————————-
#define cds A0

//서보 모터 오브젝트 생성
Servo myservo;

// 온습도 센서를 디지털 4번 핀에 연결합니다.
#define DHTPIN 8
#define DHTTYPE DHT11

//네오픽셀 연결 핀
#define PIN1 4
//네오픽셀 LED개수
#define NUMPIXELS 4
Adafruit_NeoPixel pixels1 = Adafruit_NeoPixel(NUMPIXELS, PIN1, NEO_GRB + NEO_KHZ800);
int delayval = 10;

DHT dht(DHTPIN, DHTTYPE);
SoftwareSerial Serial1(2, 3); // rx,tx

// 서보모터 변수
int val1 = 0;
int val2 = 0;

// LCD의 경우, 고유 주소를 가지고 있는데 대부분의 LCD는 2개의 주소 중 하나로 세팅되어 있다.
// 0x3F, 0×27 이렇게 두가지 주소가 있다.
LiquidCrystal_I2C lcd(0×27, 16, 2);

//—————————————

void GetData();
void get_data_print2();
void data_print1();
void LED_R1();
void LED_B1();
void LED_G1();
void window_open_close(int out, int in);
int dust_val(); // 먼지 값받아오는 함수

void setup () {

// 시리얼통신
Serial.begin(9600);
// 블루투스 통신
Serial1.begin(9600);
//서보 모터 10번핀
myservo.attach(10);
myservo.write(100);
// 조도센서
pinMode(cds, INPUT);
// 미세먼지센서
pinMode(ledPower, OUTPUT);
pinMode(measurePin, INPUT);
//온도습도센서 시작
dht.begin();
// 네오픽셀 시작
pixels1.begin(); // This initializes the NeoPixel library.

lcd.init();
lcd.backlight();
lcd.clear();
}

void loop () {

// 블루투스 통신을 통해 내부데이터를 받아서 파싱을 하고, 각 변수에 저장

if (Serial1.available()) {
GetData();
}
if (arrIndex != 0 && arrIndex < 9) {
Serial.println(“내부데이터 변수 저장”);

if (blockData[0] == 104)
refined_humidity2 = blockData[1];
if (blockData[2] == 116)
refined_temperature2 = blockData[3];
if (blockData[4] == 99)
refined_value2 = blockData[5];
if (blockData[6] == 100)
refined_dust_data2 = blockData[7];
} else {

Serial.println(“내부데이터 변수 이상으로 미저장 or 블루투스 미연결—–”);
}

// 습도와 온도값을 측정하고, 제대로 측정되었는지 확인해줍니다.
byte humidity = dht.readHumidity();
byte temperature = dht.readTemperature();

refined_humidity = constrain(humidity, 0, 100);
refined_temperature = constrain(temperature, 0, 100);

// 조도센서 값
int value = analogRead(cds);
refined_value = map(value, 0, 1023, 100, 0);

// 먼지센서 값
int dust_data = dust_val(); // 먼지량을 받아옴
refined_dust_data = map(dust_data, 0, 1023, 0, 100);

// 미세먼지 농도에 따라 색변화와 부저를 활용하여 경고음 제공하기.

if (refined_dust_data > 65) {
delay(10);
LED_R1();
} else if (refined_dust_data <= 65 && refined_dust_data > 50 ) {
delay(10);
LED_RB1();
} else if (refined_dust_data <= 50 && refined_dust_data > 25) {
delay(10);
LED_B1();
} else {
LED_G1();
}

// 내부데이터와 외부데이터를 비교하여, 창문개폐 진행
// 내부 공기청정기팬의 경우에는 외부,내부 미세먼지 농도에 상관없이, 내부 미세먼지가 25 이상인 경우에는 자동으로 작동이된다.
// 창문개폐의 경우는
// 1) 외부 창문이 열리는 경우
// 외부의 미세먼지농도가 내부보다 낮을 때,
// 외부 미세먼지 농도가 높은 경우에는 창문을 열리지 않고, 내부 팬에 의해서 공기정화가 진행이 된다.
// 내부 미세먼지 농도가 25 이상일 때, 동작한다.
window_open_close(refined_dust_data, refined_dust_data2);

lcd.clear();
lcd.setCursor(0, 0);
lcd.print(“out dust :”);
lcd.print(refined_dust_data);
lcd.setCursor(0, 1);
if(refined_dust_data2 ==0){
lcd.print(“BT disconnected”);
}else{
lcd.print(“inner dust :”);
lcd.print(refined_dust_data2);
}

get_data_print2();
data_print1();
}

//—————————————
void LED_R1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(255, 0, 0)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_G1() {

for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(0, 255, 0)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_B1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(0, 0, 255)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_RB1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(255, 0, 120)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

//—————————————
int dust_val() //센서의 노이즈 값으로인해 여러번 데이터를 받은뒤 평균 먼지량을 구함
{
int dust = 0;
for (int i = 0; i < SampleTime; i++)
{
digitalWrite(ledPower, LOW); // power on the LED 0.028초부터 led가 가장 밝음으로 최소샘플링 타임으로 결정
delayMicroseconds(samplingTime);

voMeasured = analogRead(measurePin); // read the dust value
delayMicroseconds(deltaTime);
digitalWrite(ledPower, HIGH); // turn the LED off
delayMicroseconds(sleepTime);
//////////////////////////////////////////// dust vlaue 받아오기
calcVoltage = voMeasured * (5.0 / 1024.0);
dustDensity = calcVoltage / 0.005; //(0.17 * calcVoltage – 0.1) * 1000;
/////////////////////value 의 미세한 변화를 수식으로 먼지량으로 변환
if (dustDensity < 0) { // value 의 값이 마이너스일경우 오류 임으로 0을 대입
dustDensity = 0;
}
dust += dustDensity;
}
return (dust / SampleTime)+50; // 센싱한값의 평균을 return
}

void GetData() {
Serial.println();
Serial.println(“데이터 가져오기 시작 “);
char temp;
arrIndex = 0;

temp = (char)Serial1.read();
Serial.println(“”);
Serial.print(temp);

// 전송 시작 확인
// 블루투스 시작을 알리는 문자를 s로 설정 !!!
if (temp != ‘s’) {
while (temp != ‘e’ ) {
temp = (char)Serial1.read();
}
return;
}
String stringTemp;

while (true) {
// Serial.print(“arrIndex : “);
// Serial.println(arrIndex);

if (Serial1.available()) {
temp = (char)Serial1.read();
Serial.print(temp);

if (arrIndex > 10) {
// Serial.println(“내부데이터 변수 이상 ———-”);
break;
}

// ‘_’ 라는 문자를 통해 구분을 지어준다. 구분을 지어주는 표시라고 생각을 하면된다. ———–
if (temp != ‘_’) {

// ‘e’ 라는 문자가 나오게 되면 블루투스 통신이 끝났다는 의미이다.
if (temp == ‘e’) {
break;
}

if (isAlpha(temp)) { //temp라는 변수가 문자인지 확인을 한다.

blockData[arrIndex] = temp;
arrIndex++;

} else if (isDigit(temp)) { //temp라는 변수가 숫자인지 확인을 한다.
stringTemp += temp;

} else {
// 원하지 않는 데이터 값
Serial.println(“upload failed. error: not digit or alpha”);
arrIndex = 0;
return;
}
}

else if (temp == ‘_’) {
if (stringTemp == “”)
continue; // 아래의 것들을 건너뛴다.
blockData[arrIndex] = stringTemp.toInt();
//변수 초기화
stringTemp = “”;
arrIndex++;
}
}
}
Serial.println(“끝”);
Serial.println(“”);
delay(30);
}

// int out은 외부미세먼지농도, int in 내부미세먼지농도
void window_open_close(int out, int in) {

// 내부미세먼지농도가 낮아서 공기정화를 할 필요없는 경우
if(in < 3){
return;}
if (in < 25) {
Serial.println(“내부 공기가 매우 깨끗합니다.”);
val1 = 100;
if (val1 != val2) {
myservo.write(val1); // 창문이 닫힘.
val2 = val1;
delay(10);
}
return;
}
// 내부미세먼지농도가 안좋은 경우
else {
if (out < in) {
Serial.println(“환기를 위해 창문 open. “);
val1 = 0;
if (val1 != val2) {
myservo.write(val1); // 창문이 닫힘.
val2 = val1;
delay(10);
}
delay(10);
} else {
Serial.println(“외부 미세먼지 농도가 높아, 창문 close. “);
val1 = 100;
if (val1 != val2) {
myservo.write(val1); // 창문이 닫힘.
val2 = val1;
delay(10);
}
delay(10);
}
}
}

void data_print1() {
Serial.print(“외부센서데이터 : “);
Serial.print(“humidity : “);
Serial.print(refined_humidity);
Serial.print(” temp :”);
Serial.print(refined_temperature);
Serial.print(” cds : “);
Serial.print(refined_value);
Serial.print(” dust : “);
Serial.println(refined_dust_data);
delay(300);
}

void get_data_print2() {
Serial.print(“내부센서데이터 : “);
Serial.print(“humidity : “);
Serial.print(refined_humidity2);
Serial.print(” temp :”);
Serial.print(refined_temperature2);
Serial.print(” cds : “);
Serial.print(refined_value2);
Serial.print(” dust : “);
Serial.println(refined_dust_data2);
delay(300);
아두이노 코드 2 – 내부 담당 코드
#include “SoftwareSerial.h”
#include <Adafruit_NeoPixel.h>
#include “DHT.h”
// I2C LCD를 쉽게 제어하기 위한 라이브러리를 추가해줍니다.
#include <LiquidCrystal_I2C.h>

/*미세먼지센서 변수들*/
#define measurePin A1 //Connect dust sensor to Arduino A0 pin
#define ledPower 6 //Connect 3 led driver pins of dust sensor to Arduino D4
#define SampleTime 30 //먼지센서 30회 샘플링

int samplingTime = 280;
int deltaTime = 40;
int sleepTime = 9680;
float voMeasured = 0;
float calcVoltage = 0;
float dustDensity = 0;
// 정제된 변수값
byte refined_humidity = 0;
byte refined_temperature = 0;
byte refined_value = 0;
byte refined_dust_data = 0;

//조도센서——————————–
#define cds A0

//부저 ———————————
#define buzzer 7

//모터팬 ——————————
#define fan 9

// 온습도 센서를 디지털 4번 핀에 연결합니다.
#define DHTPIN 8
#define DHTTYPE DHT11

//——————————-

//네오픽셀 연결 핀
#define PIN1 4

//네오픽셀 LED개수
#define NUMPIXELS 4
Adafruit_NeoPixel pixels1 = Adafruit_NeoPixel(NUMPIXELS, PIN1, NEO_GRB + NEO_KHZ800);
int delayval = 10;
int color_value=0;

DHT dht(DHTPIN, DHTTYPE);
SoftwareSerial Serial1(2, 3); // rx,tx

// LCD의 경우, 고유 주소를 가지고 있는데 대부분의 LCD는 2개의 주소 중 하나로 세팅되어 있다.
// 0x3F, 0×27 이렇게 두가지 주소가 있다.
LiquidCrystal_I2C lcd(0×27, 16, 2);

//—————————————-
void SendData();
void LED_RB1();
void setup () {

// 시리얼통신
Serial.begin(9600);
// 블루투스 통신
Serial1.begin(9600);
// 조도센서
pinMode(cds, INPUT);
// 미세먼지센서
pinMode(ledPower, OUTPUT);
pinMode(measurePin, INPUT);
//온도습도센서 시작
dht.begin();
// 12V 모터팬
pinMode(fan, OUTPUT);
// 네오픽셀 시작
pixels1.begin(); // This initializes the NeoPixel library.

// 부저 // #define buzzer 7
pinMode(buzzer, OUTPUT);
lcd.init();
lcd.backlight();
lcd.clear();
}
int dust_val(); // 먼지 값받아오는 함수
void loop () {

// 습도와 온도값을 측정하고, 제대로 측정되었는지 확인해줍니다.
byte humidity = dht.readHumidity();
byte temperature = dht.readTemperature();

refined_humidity = constrain(humidity, 0, 100);
refined_temperature = constrain(temperature, 0, 100);

// 조도센서 값
int value = analogRead(cds);
refined_value = map(value, 0, 1023, 100, 0);
color_value=map(refined_value, 10, 100, 0, 255);

// 먼지센서 값
int dust_data = dust_val(); // 먼지량을 받아옴
refined_dust_data = map(dust_data, 0, 1023, 0, 100);

Serial.print(” temp : “);
Serial.print(refined_temperature);
Serial.print(“‘c “);
Serial.print(” humidity : “);
Serial.print(refined_humidity);
Serial.print(“% “);
Serial.print(” cds : “);
Serial.print(refined_value);
Serial.print(” dust : “);
Serial.println(refined_dust_data);

lcd.clear();
lcd.setCursor(0, 0);
lcd.print(“hm:”);
lcd.print(refined_humidity);
lcd.print(” tp:”);
lcd.print(refined_temperature);
lcd.setCursor(0, 1);
lcd.print(“cds :”);
lcd.print(refined_value);
lcd.print(” dust :”);
lcd.print(refined_dust_data);

// 미세먼지 농도에 따라 색변화와 부저를 활용하여 경고음 제공하기.

if (refined_dust_data > 68) {
analogWrite(fan,150);
delay(10);
LED_R1();
tone(buzzer, 1000);
} else if (refined_dust_data <= 68 && refined_dust_data > 50 ) {
analogWrite(fan,150);
noTone(buzzer);
delay(10);
LED_RB1();
} else if (refined_dust_data <= 50 && refined_dust_data > 25) {
analogWrite(fan, 120);
noTone(buzzer);
delay(10);
LED_B1();
} else {
analogWrite(fan, 0);
noTone(buzzer);
LED_G1();
}

SendData();
}

//——————-

void LED_R1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(color_value, 0, 0)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_RB1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(color_value, 0, color_value/2)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_G1() {

for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(0, color_value, 0)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void LED_B1() {
for (int i = 0; i < NUMPIXELS; i++) {
pixels1.setPixelColor(i, pixels1.Color(0, 0, color_value)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}

void rain_color1() {

for (int i = 0; i < NUMPIXELS; i++) {
int a = random(0, 255);
int b = random(0, 255);
int c = random(0, 255);

pixels1.setPixelColor(i, pixels1.Color(a, b, c)); // Moderately bright green color.
pixels1.show(); // This sends the updated pixel color to the hardware.
delay(delayval); // Delay for a period of time (in milliseconds).
}
}
//int dust_val() //센서의 노이즈 값으로인해 여러번 데이터를 받은뒤 평균 먼지량을 구함
{
int dust = 0;
for (int i = 0; i < SampleTime; i++)
{
digitalWrite(ledPower, LOW); // power on the LED 0.028초부터 led가 가장 밝음으로 최소샘플링 타임으로 결정
delayMicroseconds(samplingTime);

voMeasured = analogRead(measurePin); // read the dust value
delayMicroseconds(deltaTime);
digitalWrite(ledPower, HIGH); // turn the LED off
delayMicroseconds(sleepTime);
////////////////////////////////// dust vlaue 받아오기
calcVoltage = voMeasured * (5.0 / 1024.0);
dustDensity = calcVoltage / 0.005; //(0.17 * calcVoltage – 0.1) * 1000;
////////////value 의 미세한 변화를 수식으로 먼지량으로 변환
if (dustDensity < 0) { // value 의 값이 마이너스일경우 오류 임으로 0을 대입
dustDensity = 0;
}
dust += dustDensity;
}
return dust / SampleTime; // 센싱한값의 평균을 return
}

void SendData() {
Serial1.print(“s_”);
Serial1.print(“h_”);
Serial1.print(refined_humidity);
delay(3);
Serial1.print(“_t_”);
Serial1.print(refined_temperature);
delay(3);
Serial1.print(“_c_”);
Serial1.print(refined_value);
delay(3);
Serial1.print(“_d_”);
delay(3);
Serial1.print(“_e”);
delay(200);
}

FINDTABLE

void lineTracing()
{
/* 0.5초마다 라인트레이서 모듈 센서 측정 */
if((millis() – time) >= SENSING_PERIOD)
{
time = millis();
getIRSensor();
// showIRSensor();
if(isCrosswalk()){
Serial.println(F(“갈림길 감지”));
crosswalkCnt += 1;
if(isTurningTime(crosswalkCnt)){//회전할 갈림길
if(turn[target_table] == LEFT)
Serial.println(F(“왼쪽”));
else if(turn[target_table] == RIGHT){
Serial.println(F(“오른쪽”));
}
moveTracer(turn[target_table]);//회전한다.
}else{
Serial.println(F(“갈림길 통과”));
moveTracer(FORWARD_THROUGH_CROSSWALK);//갈림길 통과
}

}else if(isRoad()){
Serial.println(F(“straight”));
moveTracer(STRAIGHT);//앞으로 이동.
}else{
Serial.println(F(“stop”));
moveTracer(STOP);//멈춘다.
isDone = true;
}
}

}

bool isCrosswalk(){
return !RightOut && !LeftOut;//검검이면 갈림길!!
}

bool isTurningTime(int cnt){
Serial.println(cnt);
return cnt == crosswalk[target_table];
}

bool isRoad(){
if (!LeftOut || !LeftIn || !RightIn || !RightOut)
{
cnt_isRoad = 0;
return true;
}
else
{
if (cnt_isRoad != 5){cnt_isRoad += 1; return true;}
if (cnt_isRoad == 5){cnt_isRoad = 0; return false;}
}
}

void moveTracer(int dir){
// Serial.print(“moveTracer”);
// Serial.println(dir);

switch(dir){
case STRAIGHT : moveForward();break;
case LEFT : turnLeft();break;
case RIGHT : turnRight();break;
case STOP : stopMoving();break;
case FORWARD_THROUGH_CROSSWALK : moveThroughCrosswalk();break;
}
}
/* 라인트레이서 모듈을 사용하여 검은색 선을 따라가는 함수 */
void moveForward()
{
if (!LeftIn && !RightIn)
{
// Serial.println(“forward 11″);
pidControl_Hz(Hz, 20);
}
else if (LeftIn && !RightIn)
{
// Serial.println(“forward 01″);
MotorA(STRAIGHT, STRAIGHT_SPEED_WEEK);
MotorB(STRAIGHT, STRAIGHT_SPEED_STRONG);
}
else if (!LeftIn && RightIn)
{
// Serial.println(“forward 10″);
MotorA(STRAIGHT, STRAIGHT_SPEED_STRONG);
MotorB(STRAIGHT, STRAIGHT_SPEED_WEEK);
}
else
{
// Serial.println(“forward xx”);
pidControl_Hz(Hz, 20);
}

}

void turnLeft()
{
// Serial.println(“turn left”);

while(!RightOut || !LeftOut){//하양이면 끝. 도는 동안은 계속 검정.
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}
Serial.println(F(“왼쪽~~~~~~~~~~~~~~~~”));
MotorA(STRAIGHT, TURN_SPEED);
MotorB(STRAIGHT, 0);
}
Serial.println(F(“왼쪽~~~~~~~~~~~~끝~~~~”));

}
void turnRight()
{
// Serial.println(“turn right”);

while(!RightOut || !LeftOut){//하양이면 끝. 도는 동안은 계속 검정.
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}
// if(LeftOut != pre){
// chgCnt += 1;
// pre = LeftOut;
// }
Serial.println(F(“오른쪽~”));

MotorA(STRAIGHT, 0);
MotorB(STRAIGHT, TURN_SPEED);
}
Serial.println(F(“오른끝쪽”));
}
void stopMoving()
{
// Serial.println(“stop”);
MotorA(STOP, 0);
MotorB(STOP, 0);
}

void moveThroughCrosswalk(){
// Serial.println(“move Through crosswalk”);
while(isCrosswalk()){
if((millis() – time) >= SENSING_PERIOD){
time = millis();
getIRSensor();
}

MotorA(STRAIGHT, STRAIGHT_SPEED);
MotorB(STRAIGHT, STRAIGHT_SPEED);
delay(100);
}
}
/****************************/

IRSensor

/* 프로그램 내용 : bfd-1000 IR센서 관련 함수
*
* getIRSensor IR센서 값 읽어오는 함수
* getIRSensor_Hz 주어진 Hz 속도로 IR센서 값 읽어오는 함수
* showIRSensorChar IR센서 값을 문자로 시리얼모니터로 출력해주는 함수
* showIRSensorValue IR센서 값을 시리얼모니터로 출력해주는 함수
*
*/

void getIRSensor()
{
LeftOut = digitalRead(SS1_LEFT_OUT);
LeftIn = digitalRead(SS2_LEFT_IN);
//Center = digitalRead(SS3_CENTER);
RightIn = digitalRead(SS4_RIGHT_IN);
RightOut = digitalRead(SS5_RIGHT_OUT);
//Bump = digitalRead(CLP_BUMP);
//Near = digitalRead(NEAR);
}

void getIRSensor_Hz(int Hz)
{
if( (millis()-time) % (1000 / Hz) == 0 )
{
getIRSensor();
}
}

void showIRSensorChar()
{
if (LeftOut == 1) {Serial.print(“L “);} else {Serial.print(“- “);}
if (LeftIn == 1) {Serial.print(“l “);} else {Serial.print(“- “);}
//if (Center == 1) {Serial.print(“C “);} else {Serial.print(“- “);}
if (RightIn == 1) {Serial.print(“r “);} else {Serial.print(“- “);}
if (RightOut == 1) {Serial.print(“R “);} else {Serial.print(“- “);}
//if (Bump == 1) {Serial.print(” BUMP!”);} else {Serial.print(” “);}
//if (Near == 1) {Serial.print(” NEAR”);} else {Serial.print(” “);}
Serial.println();
}

void showIRSensorValue()
{
Serial.print(“IRSensor : “);
Serial.print(LeftOut);Serial.print(LeftIn);
Serial.print(RightIn);Serial.print(RightOut);
Serial.println();
}

main_ver3

#include “WiFiEsp.h”
#include <MySQL_Connection.h>
#include <MySQL_Cursor.h>
#include “SoftwareSerial.h”

/* Arduino Pin */
#define SS1_LEFT_OUT A1
#define SS2_LEFT_IN A2
#define SS4_RIGHT_IN A3
#define SS5_RIGHT_OUT A4

#define encoderPinA 2
#define encoderPinB 3

#define MotorA1 5
#define MotorA2 6
#define MotorB1 10
#define MotorB2 11
#define TURN_SPEED 200
#define STRAIGHT_SPEED 110

#define STRAIGHT_SPEED_STRONG 200
#define STRAIGHT_SPEED_WEEK 110

#define SENSING_PERIOD 100

/* global variable – IR sensor */
byte LeftOut;
byte LeftIn;
byte RightIn;
byte RightOut;

/* global variable – ServingRobot Direction */
int cmd_direction = 0;
int cnt_isRoad = 0;

/* 서빙로봇 명령 */
const int STOP = 0;
const int LEFT = 1;
const int STRAIGHT = 2;
const int RIGHT = 3;
const int BACK = 4;
const int FORWARD_THROUGH_CROSSWALK = 5;
/* global variable – PID Control */
long encoderPos = 0;
double angle = 0, angle_pre=0;
int time1 = 0;
int value = 0;

double v;
double v_pre = 0;
float Kp = 4.2227;
float Ki = 2.29743;
float Kd = -0.03976;
float Ke = 0.084;

/*
float Kp = 1.674;
float Ki = 0.8239;
float Kd = -0.3584;
float Ke = 0.084;
*/
int k = 0;

double PControl, IControl=0, DControl, PIDControl;
double error, error_pre;
int pwm_in;

/* global variable – WiFi */
long target_table = 0;

/* global variable – Find Table */
int crosswalk[9] = {0, 1, 1, 2, 2, 3, 3, 4, 4};//갈림길 지나야하는 횟수. index 0은 사용하지 않는다.
int turn[9] = {0, LEFT, RIGHT, LEFT, RIGHT, LEFT, RIGHT, LEFT, RIGHT};//회전해야할 때, left로 가야하는지 right로 가야하는지
int crosswalkCnt = 0;//몇 번째 갈림길을 만났는지 체크!
bool isDone = true;

WiFiEspClient client;
MySQL_Connection conn((Client *)&client);
// Create an instance of the cursor passing in the connection
MySQL_Cursor cur = MySQL_Cursor(&conn);
MySQL_Cursor *cur_mem = new MySQL_Cursor(&conn);

SoftwareSerial esp(8, 9); // RX, TX
byte mac_addr[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };

IPAddress server_addr(106,10,49,227); // IP of the MySQL *server* here
char user[] = “sexymandoo”; // MySQL user login username
char password[] = “sexymandoo”; // MySQL user login password

char ssid[] = “Sexy_Jaewon”; // 공유기 이름 SSID
char pass[] = “jaewonsexy”; // 공유기 암호 Password
// Sample query
char query[] = “SELECT _table FROM project.orderDB WHERE call_arduino = ’1′ AND serving = ’0′ ;”;
char query2[] = “UPDATE project.orderDB SET serving = ’1′ WHERE call_arduino = ’1′ AND serving = ’0′ ;”;
int status = WL_IDLE_STATUS; // Status
int Hz = 20;
unsigned long time; // 시간 측정을 위한 변수
void setup() {
pinMode(encoderPinA, INPUT_PULLUP);
attachInterrupt(0, doEncoderA, CHANGE);
pinMode(encoderPinB, INPUT_PULLUP);
attachInterrupt(1, doEncoderB, CHANGE);

pinMode(MotorA1, OUTPUT);
pinMode(MotorA2, OUTPUT);
pinMode(MotorB1, OUTPUT);
pinMode(MotorB2, OUTPUT);

Serial.begin(9600);
esp.begin(9600);
time = millis();

WiFi.init(&esp);

if (WiFi.status() == WL_NO_SHIELD)
{
Serial.println(F(“WiFi shield not present”));
while (true);
}

// 와이파이 접속여부 확인
while (WiFi.status() != WL_CONNECTED)
{
Serial.print(F(“Attempting to connect to WPA SSID: “));
Serial.println(ssid);

status = WiFi.begin(ssid, pass);
}

// 와이파이 접속정보
Serial.println(F(“You’re connected to the network”));
Serial.print(F(“SSID: “));
Serial.println(WiFi.SSID());

IPAddress ip = WiFi.localIP();
Serial.print(F(“IP Address: “));
Serial.println(ip);

long rssi = WiFi.RSSI();

Serial.println(F(“Connecting to the server”));
if (conn.connect(server_addr, 3306, user, password)) {
delay(1000);
Serial.println(F(“Connected to the server”));
}
else
Serial.println(F(“Server connection failed.”));
}

void loop() {

if(target_table == 0){
get_table_number();
isDone = false;
crosswalkCnt = 0;
}
if(target_table != 0)
{
lineTracing();
}
if(isDone && (target_table != 0))
{
cur_mem->execute(query2); //MySQL에 서빙 완료를 알림
cur.close();
target_table = 0;
}

//pidControl_Hz(Hz, 30); // 30cm/s 속도로 주행
}

void get_table_number()
{
row_values *row = NULL;
target_table = 0;

delay(1000);

cur_mem = new MySQL_Cursor(&conn);
// query를 실행시켜서 MySQL에서 데이터를 읽어옴
cur_mem->execute(query);
column_names *columns = cur_mem->get_columns();

do {
row = cur_mem->get_next_row();
if (row != NULL) {

target_table = atol(row->values[0]);
}
} while (row != NULL);
// 동적 메모리 할당 지우기
delete cur_mem;

// 결과 출력
Serial.print(F(” 서빙테이블 = “));
Serial.println(target_table);

delay(500);
}

 motor

/* 프로그램 내용 : DC Motor 컨트롤하는 함수들
*
* MotorA MotorA를 PWM 전압으로 조절하는 함수
* MotorB MotorB를 PWM 전압으로 조절하는 함수
*
* doEncoderA DC모터 엔코더 카운트 하기 위한 함수
* doEncoderB DC모터 엔코더 카운트 하기 위한 함수
* pos2ang 엔코더로부터 회전 각도 구하는 함수
* pos2ang_Hz 주어진 Hz속도로 회전 각도 구하는 함수
*
*/

void MotorA(int dir, int _speed)
{
if (dir == STRAIGHT)
{
analogWrite(MotorA1, 0);
analogWrite(MotorA2, _speed);
}
else
{
analogWrite(MotorA1, 0);
analogWrite(MotorA2, 0);
}
}

void MotorB(int dir, int _speed)
{
if (dir == STRAIGHT)
{
analogWrite(MotorB1, _speed);
analogWrite(MotorB2, 0);
}
else
{
analogWrite(MotorB1, 0);
analogWrite(MotorB2, 0);
}
}

void doEncoderA()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?1:-1;
}

void doEncoderB()
{
encoderPos += (digitalRead(encoderPinA)==digitalRead(encoderPinB))?-1:1;
}

void pos2ang()
{
angle = -encoderPos/(341.2*4) * 360 /180*3.141592 ;
}

 PIDControl

void pidControl_Hz(int Hz, double input_v)
{
double t = 0.1;
pos2ang(); // 회전 각도 측정(라디안)
v = 3.15*(angle-angle_pre)/t;
angle_pre = angle;
error=(input_v-v)*Ke;
PControl=Kp*error;
if(IControl <= 4){IControl+=Ki*error*t;} //Windup 방지
//IControl+=Ki*error*t;
DControl=Kd*(error-error_pre)/t;

PIDControl=PControl+IControl+DControl;

pwm_in=255/6*PIDControl;
if(pwm_in>=255){pwm_in=255;}
else if(pwm_in<=60){pwm_in=60;}

Serial.print(“pwm_in: “); Serial.print(pwm_in);
Serial.print(” v:”); Serial.print(v);
Serial.print(” error:”); Serial.print(error/Ke);
Serial.print(” PIDControl: “); Serial.print(PIDControl);
Serial.print(” P:”); Serial.print(PControl);
Serial.print(” I:”); Serial.print(IControl);
Serial.print(” D:”); Serial.println(DControl);

//Serial.println(v);

MotorA(STRAIGHT, pwm_in);
MotorB(STRAIGHT, pwm_in);
//MotorA(STRAIGHT, -50);
//MotorB(STRAIGHT, -50);

//angle_pre = angle;
v_pre = v;
error_pre = error;
}

#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>
#include <Servo.h>

#define BLYNK_PRINT Serial

char auth[] = “GnCBNY4yVH2j95PCaNmN1QniKvU1QgfL”;

// Your WiFi credentials.
// Set password to “” for open networks.
char ssid[] = “HanNet”;
char pass[] = “chan0812″;

Servo switchServo;
int oper_angle = 22; // 집마다 설정 다름
int lightServoSwtich = 0; //중립 0, On : 1, Off : 2;
int lightServoFlag = 1; // 지속적 신호에 대한 Flag
int middleAngle = 90 + 3;

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
Blynk.begin(auth, ssid, pass);

switchServo.attach(4); //D2
switchServo.write(middleAngle); //Prevent reset move 180 degree
Serial.println(“Start!! nodeMCU_lightSwitch”);
}

BLYNK_WRITE(V0){
int pinValue = param.asInt(); // assigning incoming value from pin V1 to a variable
if(pinValue == 1){
Serial.println(“Swtich On”);
lightServoSwtich = 1;
} if(pinValue == 0) {
Serial.println(“Swtich Off”);
lightServoSwtich = 2;
}
}

BLYNK_WRITE(V1){
int pinValue = param.asInt();
oper_angle = pinValue;
Serial.print(“Servo Max Angle is Changed : “);
Serial.println(oper_angle);
}
BLYNK_WRITE(V2){
int pinValue = param.asInt();
// Serial.print(“Received Signal from Google to turn on : “);
// Serial.println(pinValue);
if(pinValue == 1){
Serial.println(“Swtich On”);
lightServoSwtich = 1;
} if(pinValue == 0) {
Serial.println(“Swtich Off”);
lightServoSwtich = 2;
}
//Serial.println(“Servo Activated!!!!!”);
}

void loop() {
// put your main code here, to run repeatedly:
Blynk.run();

if (lightServoSwtich == 1 && lightServoFlag == 1){ // light On
Serial.println(“ServoActivate : light On”);
switchServo.write(middleAngle – oper_angle);
delay(2000);
lightServoFlag = 2;
lightServoSwtich = 0;
switchServo.write(middleAngle);
} if(lightServoSwtich == 2 && lightServoFlag == 2){ // light Off
Serial.println(“ServoActivate : light Off”);
switchServo.write(middleAngle + oper_angle);
delay(2000);
lightServoFlag = 1;
lightServoSwtich = 0;
switchServo.write(middleAngle);
}
}

#from collections import deque
import cv2
import imutils
#import time
import numpy as np
waypoint = [] #로봇이 이동할 지점의 좌표
hsv = np.zeros((1,3), dtype = int)
colorUpper = (0,0,0) #color 범위 지정
colorLower = (0,0,0)
CAM_ID = 0
def mouse_callback(event, x, y, flags, param):
—————————-
마우스 콜백 함수
마우스 왼쪽 버튼 클릭할 경우 이동할 좌표를 waypoint 리스트에 추가
마우스 오른쪽 버튼 클릭할 경우 해당 마우스가 해당하는 pixel의 색깔을 추출한 후, 색상 범위 설정
마우스 휠 클릭할 경우 waypoint 에서 가장 최근의 좌표 제거(경로 취소할 때 이용)
—————————-
global waypoint, hsv, colorUpper, colorLower
if event == cv2.EVENT_LBUTTONDOWN:
waypoint.append((x,y))
if event == cv2.EVENT_RBUTTONDOWN:
color = img[1][y, x] one_pixel = np.uint8([[color]])
hsv = cv2.cvtColor(one_pixel, cv2.COLOR_BGR2HSV)
hsv = hsv[0][0] colorUpper = np.array([hsv[0] + 35, hsv[1] + 35, hsv[2] + 35])
colorLower = np.array([hsv[0] – 35, hsv[1] – 35, hsv[2] – 35])
if event == cv2.EVENT_MBUTTONDOWN:
if len(waypoint) != 0:
waypoint.pop(len(waypoint)-1)
def Tracking(frame, hsvUpper, hsvLower):
—————————-
컬러마커를 둘러싸는 원을 그리고 원의 중심을 return
(1. 지정한 색깔 범위에 포함되는 부분에 마스크를 생성한다.
2. 마스크로부터 contour를 구한 후 contour를 둘러싸는 외접원을 구한다.
3. 외접원의 반지름이 5 이상일 경우 원을 frame에 그리고, 원의 중심을 return한다. )
—————————-
frame = frame[1] frame = imutils.resize(frame, width = 600)
blurred = cv2.GaussianBlur(frame, (11,11),0)
hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)
mask = cv2.inRange(hsv, hsvLower, hsvUpper)
mask = cv2.erode(mask, None, iterations = 2)
mask = cv2.dilate(mask, None, iterations = 2)
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
cnts = imutils.grab_contours(cnts)
center = None
if len(cnts) > 0:
c = max(cnts, key=cv2.contourArea)
((x,y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"]/M["m00"]), int(M["m01"]/M["m00"]))
if radius > 5:
cv2.circle(frame, (int(x),int(y)), int(radius), (180,170,220),5)
cv2.imshow(“Frame”, frame)
#time.sleep(0.1)
return (center)
def AddTracking():
—————————-
컬러마커가 이동하여(로봇이 이동하여) 지정한 경로의 좌표에 근접하는 경우, 그 좌표를 waypoint list에서 빼낸다.
—————————-
global waypoint, X1, Y1
if len(waypoint) != 0:
for i in waypoint:
X2 = i[0] Y2 = i[1] print(waypoint)
if abs(X1-X2)<25 and abs(Y1-Y2)<25:
waypoint.pop(0)
print(waypoint)
vs=cv2.VideoCapture(CAM_ID)
cv2.namedWindow(“Frame”)
cv2.setMouseCallback(“Frame”, mouse_callback)
while True:
img = vs.read()
if Tracking(img, colorUpper , colorLower) != None:
X1, Y1 = Tracking(img, colorUpper , colorLower)
AddTracking()
key = cv2.waitKey(1) & 0xFF
if key == ord(“q”):
break
cv2.destroyAllWindows()
cs

CodeVision AVR_4족보행로봇 제어

#include <mega128.h> //codevision에서 Atmega128사용시
#include <string.h> //string 관련 함수 , sprintf
#include <stdlib.h>
#include <stdio.h>
#include <delay.h>
#include <math.h>

// 시리얼통신에서 입력한 명령을 담아두는 곳
//예: m00123(enter) -> string[0]=’m’ string[1]=’0′ … string[6]=0x0d=‘\r’(enter에 해당하는 ASCII Code값;
char string[64];

int iTimer0,iTimer2; //타이머 초기화 카운트 값
int cmd_count;
//cmd_count: command counting, 키보드입력시 입력 횟수
int count_10usec;
//count_10usec; 10usec마다 카운팅되는 변수 timer2 overflow 시
//DegMotor:각 모터의 회전 각도,
//CntMotor:각모터의 PWM Pulse 폭
int DegMotor[12],CntMotor[12];
int OffsetDegMotor[12]={0,-5,10,5,-5,-10,13,-5,-20,-3,-2,15};
//Leg Length
float La=55,Lb=77.5,Lc=27.5;
float pi=3.141592;
/*———————————–
data를 시리얼 포트로 1 바이트 송신하기
————————————*/
void sendChar(char data)
{
int i = 0;
// To send data with the USART put the data in the USART data register
UDR0 = data;

// Check to see if the global interrupts are enabled
if(SREG & 0×80)
{
// Wait until the byte is sent or we count out
while ( !(UCSR0A&0×40) && (i<10000) )
{
i++;
}
}
else // Wait until the byte is sent
while( !(UCSR0A&0×40) ); //송신완료시까지 대기

// Clear the TXCflag
UCSR0A=UCSR0A|0×40;
}

/*—————————————————
문자열 str을 문자열 맨뒤 NULL이 나올떄 까지 한바이트씩 보내기
—————————————————-*/

void sendString(char* str)
{
while(*str) sendChar(*str++);
}
/*
만일 str[ ] = “rs232c”라면
str[0]=’r'=0×72
str[1]=’s’
str[2]=’2′=0×32
str[3]=’3′
str[4]=’2′=0×32
str[5]=’c’
str[6]=”
‘ ‘:char
” “:string
str: str이라는 문자열이 저장된 주소
*str:str에 저장된 내용
step1:while(*str) => while(‘r’); ->while(true)
step2:sendChar(*str); =>sendChar(‘r’);
step3: str++; //주소증가
step4:while(*str) => while(‘s’); ->while(true)
step5:sendChar(*str); =>sendChar(‘s’);
step6: str++;
….
stepn:while(*str) => while(”); ->while(false)
*/

/*————————————————–
시리얼 포트 초기화
8비트 데이터, 1 stop 비트,non parity, 9600 보레이트
————————————————–*/
void rs232c_initialize(void)
{
DDRE=DDRE|0×02; //포트 E의 입출력 설정 ,PE0:0 Rx0,PE1:1 Tx0
//USART Register Init
UCSR0A=0×00;
UCSR0B=0×98; //1001 1000 //7 bit : RX Complete Interrupt Enable
//4 bit : Receiver Enable. Writing this bit to one enables the USARTn Receiver.
//3 bit : Transmitter Enable. Writing this bit to one enables the USARTn Transmitter.
//2 bit : UCSR0C 2,1 bit와 연결사용

UCSR0C=0×06; //0000 0110 //6 bit : USART Mode Select. Asynchronous Operation
//5,4 bit : Parity Mode. Disabled
//3 bit : Stop Bit Select. 1-bit
//2,1 bit : Character Size. 8-bit
//0 bit : Clock Polarity. Transmitted Data : Rising XCKn Edge
// Received Data : Falling XCKn Edge

UBRR0H=0×00; //USARTn Baud Rate Register
UBRR0L=0×67; //USARTn Baud Rate Register 9600
//UBRR0L=0×8; //USARTn Baud Rate Register 115200
}

void init_Timer()
{
//prescale=1024, input frequency=16MHz/1024=64KHz T=1/64usec
//Overflow시 timing 간격은 256/64msec=16.348msec
iTimer0=0;
//Timer2, input f=16MHz, 160count=10sec is TCNT2=256-160=96
//but from tunning process, the timer2 initialzed variable is 113
iTimer2=113;
DDRC=DDRA=0xff; //Set PORTA all output
PORTC=PORTA=0×00; //Set All PortA Low

TCCR0=0×07; //oc0 not enable, normal, 분주비 1024, pb4 일반포트로 사용
TCCR2=0×01; //0b00000101 //oc2 not enable, normal, 분주비 1, pb7 일반포트로 사용
// 0 0 00 0 010
TIMSK=0×41; //timer 0and 2 overflow interrupt enable
TCNT0=iTimer0; //time 0 초기값
TCNT2=iTimer2;
}

//Deg로 입력된 것을 Pulse Width로 변환하기
void ConvertDeg2Cnt()
{
int i;
for(i=0;i<12;i++)
CntMotor[i]=(10.58*(DegMotor[i]+OffsetDegMotor[i])+459.4)/10+3;
}

interrupt [TIM0_OVF] void timer_int0()
{
PORTC=PORTA=0xff;
count_10usec=0;
TIMSK = TIMSK | 0×40; //Timer 2 Interrupt Enable
}
interrupt [TIM2_OVF] void timer_int2()
{

TCNT2=iTimer2; //the initialzed variable for 10usec timer2 overflow interrupt
count_10usec++;
if(count_10usec == CntMotor[0] ) PORTA.0=0;
if(count_10usec == CntMotor[1] ) PORTA.1=0;
if(count_10usec == CntMotor[2]) PORTA.2=0;
if(count_10usec == CntMotor[3]) PORTA.3=0;
if(count_10usec == CntMotor[4]) PORTA.4=0;
if(count_10usec == CntMotor[5]) PORTA.5=0;
if(count_10usec == CntMotor[6]) PORTA.6=0;
if(count_10usec == CntMotor[7]) PORTA.7=0;
if(count_10usec == CntMotor[8]) PORTC.0=0;
if(count_10usec == CntMotor[9]) PORTC.1=0;
if(count_10usec == CntMotor[10]) PORTC.2=0;
if(count_10usec == CntMotor[11]) PORTC.3=0;
if(count_10usec > 300)
{
TIMSK = TIMSK & 0xbf; //Timer 2 Interrupt disable
}

}

/*———————————–
수신 받은 문자열 s[]데이터 분석
————————————*/
void parseInput(char * s)
{
int imotor;
// parse first character
switch (s[0])
{
//xx축 motor 개별 제어
//m+XX(motor no.)+xxx+’\r’
case ‘m’:
case ‘M’:
//if m11135 이면 s[1]=’1′=0×31, s[2]=’1′=0×31
//아스키코드값으로 표현된 연속된 두 숫자를 10진수로 변환 하기 imotor->1*10+1=11
imotor=(s[1]-’0′)*10+(s[2]-’0′);
//s+3부터 문자열을 ascii to integer변환
/*
s[0]=’m’
s[1]=’1′
s[2]=’1′
s[3]=’1′
s[4]=’3′
s[5]=’5′
s[6]=”
이므로 s[3[부터 끝까지 ascii로되어있는 문자를 정수로 변환하시오
DegMotor[imotor]=135;
*/
DegMotor[imotor]=atoi(s+3);
ConvertDeg2Cnt();
break;

//12개의 모터 제어 데이터 열
//d+xxxxxxxxxxxx+’\r’, x는 unsigned char data
case ‘d’:
case ‘D’:
for(imotor=0;imotor<12;imotor++)
DegMotor[imotor]=(unsigned char)s[imotor+1];
ConvertDeg2Cnt();
break;

case ‘c’:
sendString(“Your command input is ..!\r\n”);
sendString(s);
sendString(“\r\n”);
break;
case ‘e’:
sendString(“Your message input is ..!\r\n”);
sendString(s);
sendString(“\r\n”);
break;
default:
break;
}
//앞에서 받은 명령을 초기화하기
s[0] = ”;
}

/*———————————————–
하이퍼터밀널에서 입력을 하고 엔터키를 입력하면
입력 데이터 뒤에 /n 이 연이어 붙는다.
/r:Carriage Return 0x0D
/n:new line , line feed 0x0A
———————————————–*/
//카보드로 입력이 들어오면 인터럽트 발생
interrupt [USART0_RXC] void usart0_rx_isr(void) //USART0 수신 완료 인터럽트
{
string[cmd_count++] = UDR0; //rx로 받은 값을 data에 저장
// “m11135+/r”=>motor 2 angle 135
/*
string[0]=‘m’
string[1]=‘1′
string[2]=‘1′
string[3]=‘1′
string[4]=‘3′
string[5]=‘5′
string[6]=‘\r’=0x0d
cmd_count->7
*/
if(string[cmd_count-1] == ‘\r’)
{
string[cmd_count-1] = ”;
//convert to a string. 문자로 하나하나 모았다가 문자열로 변환하기위해서 뒤에 NULL()로 변경
/*
string[0]=‘m’
string[1]=‘1′
string[2]=‘1′
string[3]=‘1′
string[4]=‘3′
string[5]=‘5′
string[6]=‘’=0×00
문자열로 변경된 상태
*/

parseInput(string); //명령 분석
cmd_count = 0;
}
else if(string[cmd_count-1] == ‘c’)// ‘c’+/r
{
string[1]=”; //convert to a string
parseInput(string);
cmd_count = 0;
}
else if(string[cmd_count-1] == ‘e’)// ‘e’+/r
{
string[1]=”; //convert to a string
parseInput(string);
cmd_count = 0;
//EnableTimer0Interrupt();
}
}

void Leg1(int x,int y,int z, int * theta0,int * theta1,int * theta2)
{
int w,v;
float t0,t1,t2;

w=sqrt(x*x+y*y);
v=w-Lc;
t1=atan2(v,z)+acos((La*La+z*z+v*v-Lb*Lb)/(2*La*sqrt(z*z+v*v)));
t2=acos((La*La+Lb*Lb-z*z-v*v)/(2*La*Lb));
t0=atan2(x,y);

*theta0=t0*180/pi+90;
*theta1=90-t1*180/pi;
*theta2=t2*180/pi;
}

void main(void)
{
int i;
char buff[20];

rs232c_initialize();

cmd_count=0;

for(i=0;i<12;i++) DegMotor[i]=90;
ConvertDeg2Cnt();

init_Timer();

sendString(“rs232c connected….\r\n”);

SREG=0×80; //1000 0000 //7 bit : Global Interrupt Enable

Leg1(80,40,-15,&DegMotor[0],&DegMotor[1],&DegMotor[2]);
ConvertDeg2Cnt();
itoa(DegMotor[0],buff);
sendString(buff);
sendString(“\r\n”);
itoa(DegMotor[1],buff);
sendString(buff);
sendString(“\r\n”);
itoa(DegMotor[2],buff);
sendString(buff);
sendString(“\r\n”);
while(1)
{
}
}

 CodeVision AVR (모터제어)

#include <mega128.h> //For Atmega128 in Code Vision
#include <delay.h> //For dalay_ms, delay_us

int iTimer0,iTimer2; //Timer initialized variable
int count; //count timer2 overflow interrupt
int Degree,Motor;

interrupt [TIM0_OVF] void timer_int0()
{
PORTC=PORTA=0xff;
count=0;
TIMSK = TIMSK | 0×40; //Timer 2 Interrupt Enable
}

interrupt [TIM2_OVF] void timer_int2()
{
TCNT2=iTimer2; //the initialzed variable for 10usec timer2 overflow interrupt
count++;
if(count == Motor ) PORTA.0=0;
if(count == Motor ) PORTA.1=0;
if(count == Motor) PORTA.2=0;
if(count == Motor) PORTA.3=0;
if(count == Motor) PORTA.4=0;
if(count == Motor) PORTA.5=0;
if(count == Motor) PORTA.6=0;
if(count == Motor) PORTA.7=0;
if(count == Motor) PORTC.0=0;
if(count == Motor) PORTC.1=0;
if(count == Motor) PORTC.2=0;
if(count == Motor) PORTC.3=0;
if(count > 300)
{
TIMSK = TIMSK & 0xbf; //Timer 2 Interrupt disable
}
}
void main(void)
{
iTimer0=0;
//Timer2, input f=16MHz, 160count=10sec is TCNT2=256-160=96
//but from tunning process, the timer2 initialzed variable is 113
iTimer2=113;
DDRC=DDRA=0xff; //Set PORTA all output
PORTC=PORTA=0×00; //Set All PortA Low

TCCR0=0×07; //oc0 not enable, normal, 분주비 1024, pb4 일반포트로 사용

TCCR2=0×01; //0b00000101 //oc2 not enable, normal, 분주비 1, pb7 일반포트로 사용
// 0 0 00 0 010
//TIMSK=0xC3; //timer 0 oVerflow interrupt enable
TIMSK=0×41; //timer 0and 2 overflow interrupt enable

TCNT0=iTimer0; //time 0 초기값
TCNT2=iTimer2;

SREG=0×80; //인터럽트 인에이블

Degree=90;
while(1)
{
Motor=(10.58*Degree+459.4)/10+3;
}
}