bus_system.ino
int bell_led_1 = 22;
int bell_button_1 = 23;
int bell_test_1 = 24;
int bell_led_2 = 26;
int bell_button_2 = 27;
int bell_test_2 = 28;
int bell_led_3 = 30;
int bell_button_3 = 31;
int bell_test_3 = 32;
int bell_led_4 = 34;
int bell_button_4 = 35;
int bell_test_4 = 36;
int bell_led_5 = 38;
int bell_button_5 = 39;
int bell_test_5 = 40;
int door_button = 42;
boolean bell_signal_1;
boolean bell_signal_2;
boolean bell_signal_3;
boolean bell_signal_4;
boolean bell_signal_5;
boolean door_signal;
boolean total_signal;
boolean state;

void setup() {
pinMode(bell_led_1, OUTPUT);
pinMode(bell_led_2, OUTPUT);
pinMode(bell_led_3, OUTPUT);
pinMode(bell_led_4, OUTPUT);
pinMode(bell_led_5, OUTPUT);
pinMode(bell_button_1, INPUT);
pinMode(bell_button_2, INPUT);
pinMode(bell_button_3, INPUT);
pinMode(bell_button_4, INPUT);
pinMode(bell_button_5, INPUT);
pinMode(bell_test_1, OUTPUT);
pinMode(bell_test_2, OUTPUT);
pinMode(bell_test_3, OUTPUT);
pinMode(bell_test_4, OUTPUT);
pinMode(bell_test_5, OUTPUT);
pinMode(door_button, INPUT);
Serial.begin(115200);
Serial.println(“Setting done….”);
normal_state();
}

void normal_state() {
digitalWrite(bell_led_1, LOW);
digitalWrite(bell_led_2, LOW);
digitalWrite(bell_led_3, LOW);
digitalWrite(bell_led_4, LOW);
digitalWrite(bell_led_5, LOW);
digitalWrite(bell_test_1, HIGH);
digitalWrite(bell_test_2, HIGH);
digitalWrite(bell_test_3, HIGH);
digitalWrite(bell_test_4, HIGH);
digitalWrite(bell_test_5, HIGH);
state = 0;
}

void pushed_state() {
digitalWrite(bell_led_1, HIGH);
digitalWrite(bell_led_2, HIGH);
digitalWrite(bell_led_3, HIGH);
digitalWrite(bell_led_4, HIGH);
digitalWrite(bell_test_1, LOW);
digitalWrite(bell_test_2, LOW);
digitalWrite(bell_test_3, LOW);
digitalWrite(bell_test_4, LOW);
digitalWrite(bell_test_5, LOW);
state = 1;
}

void read_signal() {
bell_signal_1 = digitalRead(bell_button_1);
bell_signal_2 = digitalRead(bell_button_2);
bell_signal_3 = digitalRead(bell_button_3);
bell_signal_4 = digitalRead(bell_button_4);
bell_signal_5 = digitalRead(bell_button_5);
door_signal = digitalRead(door_button);
Serial.println(“b1, b2, b3, b4, b5, Door”);
Serial.print(bell_signal_1);
Serial.print(” “);
Serial.print(bell_signal_2);
Serial.print(” “);
Serial.print(bell_signal_3);
Serial.print(” “);
Serial.print(bell_signal_4);
Serial.print(” “);
Serial.print(bell_signal_5);
Serial.print(” “);
Serial.println(door_signal);
total_signal = bell_signal_1||bell_signal_2||bell_signal_3||bell_signal_4||bell_signal_5;
}

void self_holding () {
if (state==0) {
if (total_signal == 1) {
pushed_state();
Serial.println(“bell pushed….”);
}
}
else if (state == 1) {
if (door_signal == 0) {
normal_state();
Serial.println(“door opend….”);
}
}
}

void loop() {
read_signal();
self_holding();
delay(100);
}

wifi_module.ino
#include <ESP8266WiFi.h>
#include <WiFiClient.h>
#include <ESP8266WebServer.h>

#ifndef APSSID
#define APSSID “ICT_TEST”
#define APPSK “ict_test”
#endif

const char *ssid = APSSID;
const char *password = APPSK;

ESP8266WebServer server(80);

uint8_t LED_PIN = 4;
bool LED_PIN_STATUS = LOW;
void handleRoot() {
LED_PIN_STATUS = LOW;
server.send(200, “text/html”, “<h1>You are connected</h1>”);
}

void handleRoot_on() {
LED_PIN_STATUS = HIGH;
server.send(200, “text/html”, “<h1> LED ON </h1>”);
}

 

void setup() {
delay(1000);
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, 0);

Serial.begin(115200);
Serial.println();
Serial.print(“Configuring access point…”);
WiFi.softAP(ssid, password);
IPAddress myIP = WiFi.softAPIP();
Serial.print(“AP IP address: “);
Serial.println(myIP);

server.on(“/”, handleRoot);
server.on(“/on”, handleRoot_on);
server.begin();
Serial.println(“HTTP server started”);
}

void loop() {
server.handleClient();
if(LED_PIN_STATUS) {
digitalWrite(LED_PIN, HIGH);
delay(200);
digitalWrite(LED_PIN, LOW);
LED_PIN_STATUS =!LED_PIN_STATUS;
}
else
{digitalWrite(LED_PIN, LOW);}
}

AndroidManifest.xml
<?xml version=”1.0″ encoding=”utf-8″?>
<manifest xmlns:android=”http://schemas.android.com/apk/res/android”
package=”com.example.pleaseplease”>
<uses-permission android:name=”android.permission.ACCESS_WIFI_STATE” />
<uses-permission android:name=”android.permission.CHANGE_WIFI_STATE” />
<uses-permission android:name=”android.permission.ACCESS_FINE_LOCATION”/>
<uses-permission android:name=”android.permission.ACCESS_COARSE_LOCATION”/>
<application
android:allowBackup=”true”
android:icon=”@mipmap/ic_launcher”
android:label=”@string/app_name”
android:roundIcon=”@mipmap/ic_launcher_round”
android:supportsRtl=”true”
android:theme=”@style/AppTheme”>
<activity android:name=”.MainActivity”>
<intent-filter>
<action android:name=”android.intent.action.MAIN” />
<category android:name=”android.intent.category.LAUNCHER” />
</intent-filter>
</activity>
</application>
</manifest>
activity_main.xml
<?xml version=”1.0″ encoding=”utf-8″?>
<LinearLayout xmlns:android=”http://schemas.android.com/apk/res/android”
xmlns:app=”http://schemas.android.com/apk/res-auto”
xmlns:tools=”http://schemas.android.com/tools”
android:layout_width=”match_parent”
android:layout_height=”match_parent”
android:orientation=”vertical”
tools:context=”.MainActivity”>

<Button
android:id=”@+id/button”
android:layout_width=”match_parent”
android:layout_height=”48dp”
android:text=”STOP”
app:layout_constraintEnd_toEndOf=”parent”
app:layout_constraintStart_toStartOf=”parent”
app:layout_constraintTop_toTopOf=”parent” />
</LinearLayout>
MainActivity.java
package com.example.pleaseplease;
import androidx.appcompat.app.AppCompatActivity;
import android.content.BroadcastReceiver;
import android.content.Context;
import android.content.Intent;
import android.content.IntentFilter;
import android.net.Uri;
import android.net.wifi.ScanResult;
import android.net.wifi.WifiConfiguration;
import android.net.wifi.WifiInfo;
import android.net.wifi.WifiManager;
import android.os.Bundle;
import android.provider.Settings;
import android.util.Log;
import android.view.View;
import android.webkit.WebChromeClient;
import android.webkit.WebSettings;
import android.webkit.WebView;
import android.webkit.WebViewClient;
import android.widget.ArrayAdapter;
import android.widget.Button;
import android.widget.ListView;
import android.widget.Toast;
import java.io.IOException;
import java.lang.reflect.Method;
import java.net.HttpURLConnection;
import java.net.MalformedURLException;
import java.net.URL;
import java.util.ArrayList;
import java.util.List;

public class MainActivity extends AppCompatActivity {
private WebView webview;
private String networkSSID = “ICT_TEST”;
private String networkPass = “ict_test”;
private WifiManager wifiManager;
private WifiConfiguration wifiConfiguration;
private int size = 0;0
private ArrayList<String> arrayList = new ArrayList<>();
private ArrayAdapter<String> adapter;
private static final String TAG = “MyActivity”;
Intent myIntent = new Intent(Settings.ACTION_LOCATION_SOURCE_SETTINGS);

@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
Button buttonScan = findViewById(R.id.button);
buttonScan.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View v) {
scanWifi();
if (wifiManager.isWifiEnabled()) {
String url = “http://192.168.4.1/on”;
Intent intent = new Intent(Intent.ACTION_VIEW, Uri.parse(url));
startActivity(intent);
}
}
});
wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE);
assert wifiManager != null;
if (!wifiManager.isWifiEnabled()) {
Toast.makeText(this, “wifi is disabled… connecting…”, Toast.LENGTH_SHORT).show();
wifiManager.setWifiEnabled(true);
}
adapter = new ArrayAdapter<>(this, android.R.layout.simple_list_item_1, arrayList);
scanWifi();
}

private void scanWifi() {
arrayList.clear();
registerReceiver(wifiReceiver, new IntentFilter(WifiManager.SCAN_RESULTS_AVAILABLE_ACTION));
wifiManager.startScan();
Toast.makeText(this, “Scanning…..”, Toast.LENGTH_SHORT).show();
}

BroadcastReceiver wifiReceiver = new BroadcastReceiver() {
@Override
public void onReceive(Context context, Intent intent) {
List<ScanResult> results = wifiManager.getScanResults();
Log.i(TAG, String.format(“size—-%d”, results.size()));
unregisterReceiver(this);
for (ScanResult scanResult : results) {
Log.i(TAG, String.format(“—-%s”, scanResult.SSID));
arrayList.add(scanResult.SSID);
adapter.notifyDataSetChanged();
if (scanResult.SSID.equals(networkSSID)) {
WifiConfiguration wifiConfiguration = new WifiConfiguration();
wifiConfiguration.SSID = networkSSID;
wifiConfiguration.preSharedKey = networkPass;
wifiConfiguration.allowedAuthAlgorithms.set(WifiConfiguration.AuthAlgorithm.OPEN);
int netId = wifiManager.addNetwork(wifiConfiguration);
Log.i(TAG, String.format(“netId = = = =%s”, netId));
if (netId == -1) {
existconnect();
Log.i(TAG, String.format(“connect to %s”, scanResult.SSID));
wifiManager.disconnect();
wifiManager.enableNetwork(netId, true);
wifiManager.reconnect();
} else {
Log.i(TAG, String.format(“connect to %s”, scanResult.SSID));
wifiManager.disconnect();
wifiManager.enableNetwork(netId, true);
wifiManager.reconnect();
}
}
}
}
};

public void existconnect() {
WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE);
assert wifiManager != null;
List<WifiConfiguration> wifiConfigurations = wifiManager.getConfiguredNetworks();
if (wifiConfigurations != null) {
for (WifiConfiguration configuration : wifiConfigurations) {
int id = configuration.networkId;
String ssid = configuration.SSID;
Log.i(TAG, String.format(“NW———-%3d %s”, id, ssid));
if (ssid.equals(networkSSID)) {
Log.i(TAG, String.format(“coneect to %3d %s”, id, ssid));
WifiConfiguration wifiConfiguration = new WifiConfiguration();
wifiConfiguration.SSID = ssid;
wifiConfiguration.preSharedKey = networkPass;
wifiConfiguration.allowedAuthAlgorithms.set(WifiConfiguration.AuthAlgorithm.OPEN);
wifiManager.disconnect();
wifiManager.enableNetwork(id, true);
wifiManager.reconnect();
}
}
}
}
}

%% Simulate One Finger
clear, clc;
SimMin = 0;
SimMax = 30;
Res = 20;

BobinR = 5;
PlateR = 11;
PlateHallR = 8;
PlateOffset = [0 0 0 0]+1;
SHAFTLENGTH = [15 50 35 25];

disp(['Simuation from ' num2str(SimMin) ' to ' num2str(SimMax) ', with ' num2str(Res) 'steps']);
sim = linspace(SimMin, SimMax, Res);
counter = 1;
clear LWs;
LWs = zeros(6,length(sim));
for i = 1:length(sim)
[SHAFT, WIRE, LW] = OneFinger(eye(4),[sim(i) sim(i) sim(i) 0 0 0 0 0 0],PlateHallR,PlateOffset,SHAFTLENGTH);
LWs(:,counter) = LW;
figure(1);
plot3(SHAFT(1,:),SHAFT(2,:),SHAFT(3,:),’-o’,'LineWidth’,3), hold on;
plot3(WIRE{1}(1,:),WIRE{1}(2,:),WIRE{1}(3,:),’-.d’,'LineWidth’,2);
plot3(WIRE{2}(1,:),WIRE{2}(2,:),WIRE{2}(3,:),’-.d’,'LineWidth’,2);
plot3(WIRE{3}(1,:),WIRE{3}(2,:),WIRE{3}(3,:),’-.d’,'LineWidth’,2);
plot3(WIRE{4}(1,:),WIRE{4}(2,:),WIRE{4}(3,:),’-.d’,'LineWidth’,2);
plot3(WIRE{5}(1,:),WIRE{5}(2,:),WIRE{5}(3,:),’-.d’,'LineWidth’,2);
plot3(WIRE{6}(1,:),WIRE{6}(2,:),WIRE{6}(3,:),’-.d’,'LineWidth’,2);
plotCircle3D(SHAFT(:,1)’+PlateOffset(1)*(SHAFT(:,2)-SHAFT(:,1))’/norm((SHAFT(:,2)-SHAFT(:,1))’),(SHAFT(:,2)-SHAFT(:,1))’,PlateR,2);
plotCircle3D(SHAFT(:,2)’+PlateOffset(2)*(SHAFT(:,3)-SHAFT(:,2))’/norm((SHAFT(:,3)-SHAFT(:,2))’),(SHAFT(:,3)-SHAFT(:,2))’,PlateR,2);
plotCircle3D(SHAFT(:,3)’+PlateOffset(3)*(SHAFT(:,4)-SHAFT(:,3))’/norm((SHAFT(:,4)-SHAFT(:,3))’),(SHAFT(:,4)-SHAFT(:,3))’,PlateR,2);
plotCircle3D(SHAFT(:,4)’+PlateOffset(4)*(SHAFT(:,5)-SHAFT(:,4))’/norm((SHAFT(:,5)-SHAFT(:,4))’),(SHAFT(:,5)-SHAFT(:,4))’,PlateR,2);
title(‘One Finger’);
axis equal;
hold off;
drawnow;
counter = counter+1;
end

figure(2);
subplot(2,3,1), plot(sim,LWs(1,:)), axis([SimMin SimMax 0 150]),title(‘Wire 1 Length’), grid on;
subplot(2,3,2), plot(sim,LWs(2,:)), axis([SimMin SimMax 0 150]),title(‘Wire 2 Length’), grid on;
subplot(2,3,3), plot(sim,LWs(3,:)), axis([SimMin SimMax 0 150]),title(‘Wire 3 Length’), grid on;
subplot(2,3,4), plot(sim,LWs(6,:)), axis([SimMin SimMax 0 150]),title(‘Wire 6 Length’), grid on;
subplot(2,3,5), plot(sim,LWs(5,:)), axis([SimMin SimMax 0 150]),title(‘Wire 5 Length’), grid on;
subplot(2,3,6), plot(sim,LWs(4,:)), axis([SimMin SimMax 0 150]),title(‘Wire 4 Length’), grid on;

figure(3);
subplot(2,3,1), plot(sim,LWs(1,:)-LWs(6,:)), axis([SimMin SimMax -50 50]),title(‘J2 WL diff’);
subplot(2,3,2), plot(sim,LWs(2,:)-LWs(5,:)), axis([SimMin SimMax -50 50]),title(‘J1_1 WL diff’);
subplot(2,3,3), plot(sim,LWs(3,:)-LWs(4,:)), axis([SimMin SimMax -50 50]),title(‘J1_2 WL diff’);
subplot(2,3,4), plot(sim,LWs(1,:)-LWs(1,1)+LWs(6,:)-LWs(6,1)), axis([SimMin SimMax -15 15]),title(‘J2 WL error’);
subplot(2,3,5), plot(sim,LWs(2,:)-LWs(2,1)+LWs(5,:)-LWs(5,1)), axis([SimMin SimMax -15 15]),title(‘J1_1 WL error’);
subplot(2,3,6), plot(sim,LWs(3,:)-LWs(3,1)+LWs(4,:)-LWs(4,1)), axis([SimMin SimMax -15 15]),title(‘J1_2 WL error’);
disp(['Max J2 wire length error: ' num2str(max(abs(abs(LWs(1,:)-LWs(1,1)+LWs(6,:)-LWs(6,1))))) 'mm']);
disp(['Max J1_1 wire length error: ' num2str(max(abs(abs(LWs(2,:)-LWs(2,1)+LWs(5,:)-LWs(5,1))))) 'mm']);
disp(['Max J1_2 wire length error: ' num2str(max(abs(abs(LWs(3,:)-LWs(3,1)+LWs(4,:)-LWs(4,1))))) 'mm']);

figure(4);
LWCs = LWs-LWs(:,1);
LWCd = rad2deg(LWCs./BobinR);
MotorPos = round(LWCd./360*4096);
subplot(3,3,1), stairs(MotorPos(1,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 1′);
subplot(3,3,2), stairs(MotorPos(2,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 2′);
subplot(3,3,3), stairs(MotorPos(3,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 3′);
subplot(3,3,4), stairs(MotorPos(6,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 6′);
subplot(3,3,5), stairs(MotorPos(5,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 5′);
subplot(3,3,6), stairs(MotorPos(4,:)), axis([0 length(LWCs) -4096 4096]), title(‘Motor Pos by wire 4′);
subplot(3,3,7), stairs(MotorPos(1,:) + MotorPos(6,:)), axis([0 length(LWCs) -256 256]), title(‘J2 Motor Pos Error’);
subplot(3,3,8), stairs(MotorPos(2,:) + MotorPos(5,:)), axis([0 length(LWCs) -256 256]), title(‘J1_1 Motor Pos Error’);
subplot(3,3,9), stairs(MotorPos(3,:) + MotorPos(4,:)), axis([0 length(LWCs) -256 256]), title(‘J1_2 Motor Pos Error’);
MaxPosErr = max([max(abs(MotorPos(1,:) + MotorPos(6,:))) max(abs(MotorPos(2,:) + MotorPos(5,:))) max(abs(MotorPos(3,:) + MotorPos(4,:)))]);
disp(['Max motor position error is: ' num2str(MaxPosErr) ' encoder tics, ' num2str(MaxPosErr/4096) ' rotation, degrees: ' num2str(MaxPosErr*360/4096)]);

// CableOneFinger.cpp : This file contains the ‘main’ function. Program execution begins and ends there.
//

#if defined(__linux__) || defined(__APPLE__)
#include <fcntl.h>
#include <termios.h>
#define STDIN_FILENO 0
#elif defined(_WIN32) || defined(_WIN64)
#include <conio.h>
#endif

#include <Windows.h>
#include <stdlib.h>
#include <iostream>

#include “dynamixel_sdk.h”
#include “CableOneFinger.h”
#include “DXL_custom.h”
#include “DXL_application.h”
#include “UserFunction.h”
#include “Robotics.h”

using namespace std;
int main()
{
int dxl_comm_result = COMM_TX_FAIL;
uint8_t dxl_error = 0;

bool dxl_addparam_result = false;
bool dxl_getdata_result = false;

const int DXL_IDs[DXLS] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };
int DXL_REL_MIN_POSs[DXLS];
int DXL_REL_MAX_POSs[DXLS];
int DXL_REL_GRASP_POSs[DXLS];
fill_n(DXL_REL_MIN_POSs, DXLS, 0);
fill_n(DXL_REL_MAX_POSs, DXLS, 3000);
fill_n(DXL_REL_GRASP_POSs, DXLS, 2500);

int DXL_INIT_POSs[DXLS];
int DXL_ABS_GOAL_POSs[DXLS];
int DXL_ABS_PRE_POS_ERRs[DXLS];
int DXL_ABS_PRESENT_POSs[DXLS];
int DXL_ABS_MIN_POSs[DXLS];
int DXL_ABS_MAX_POSs[DXLS];
int DXL_ABS_GRASP_POSs[DXLS];

int16_t DXL_PRESENT_LOAD[DXLS];
bool DXL_GRASPED[DXLS] = { false };

dynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

dynamixel::GroupSyncWrite SyncWriteGoalPos(portHandler, packetHandler, ADDR_GOAL_POSITION, LEN_GOAL_POSITION);
dynamixel::GroupSyncRead SyncReadPresentPos(portHandler, packetHandler, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION);

dynamixel::GroupSyncRead SyncReadPresentLoad(portHandler, packetHandler, ADDR_PRESENT_LOAD, LEN_PRESENT_LOAD);

if (portHandler->openPort())
cout << “Succeeded to open the dynamixel!” << endl;
else
{
cout << “Failed to open the dynamixel!” << endl;
_getch();
return -1;
}

if (portHandler->setBaudRate(BAUDRATE))
cout << “Set baudrate to ” << BAUDRATE << endl;
else
{
cout << “Failed to change the baudrate!” << endl;
_getch();
return -1;
}

if (!(initializeHand(packetHandler, portHandler, SyncReadPresentPos, SyncWriteGoalPos, &dxl_comm_result, &dxl_error, &dxl_addparam_result, &dxl_getdata_result, DXL_IDs, DXL_INIT_POSs, sizeof(DXL_IDs))))
cout << “Initializing failed!” << endl;
calculateArrayArithmetic(DXL_INIT_POSs, DXL_REL_GRASP_POSs, DXL_ABS_GRASP_POSs, sizeof(DXL_IDs), ‘+’);
calculateArrayArithmetic(DXL_INIT_POSs, DXL_REL_MIN_POSs, DXL_ABS_MIN_POSs, sizeof(DXL_IDs), ‘+’);
calculateArrayArithmetic(DXL_INIT_POSs, DXL_REL_MAX_POSs, DXL_ABS_MAX_POSs, sizeof(DXL_IDs), ‘+’);
cout << “CABLE-DRIVEN ROBOT HAND Initialized!!!” << endl;

grasping(packetHandler, portHandler, SyncReadPresentPos, SyncReadPresentLoad, SyncWriteGoalPos, &dxl_comm_result, &dxl_error, &dxl_addparam_result, &dxl_getdata_result, DXL_IDs, DXL_ABS_GRASP_POSs, DXL_ABS_PRESENT_POSs, DXL_ABS_PRE_POS_ERRs, DXL_PRESENT_LOAD, DXL_GRASPED, sizeof(DXL_IDs), DXL_MOVING_POS_THRESHOLD);
Sleep(5000);
toggleDXLTorque(packetHandler, portHandler, &dxl_comm_result, &dxl_error, DXL_IDs, sizeof(DXL_IDs), TORQUE_DISABLE);

portHandler->closePort();
return 0;
}

// Run program: Ctrl + F5 or Debug > Start Without Debugging menu
// Debug program: F5 or Debug > Start Debugging menu

// Tips for Getting Started:
// 1. Use the Solution Explorer window to add/manage files
// 2. Use the Team Explorer window to connect to source control
// 3. Use the Output window to see build output and other messages
// 4. Use the Error List window to view errors
// 5. Go to Project > Add New Item to create new code files, or Project > Add Existing Item to add existing code files to the project
// 6. In the future, to open this project again, go to File > Open > Project and select the .sln file

Raspberry Pi 2Pin = Register 1K pin = Switch first pin
Raspberry Pi 4pin = ultrasonic sensor Vcc pin
Raspberry Pi 5pin = Switch second pin
Raspberry Pi 6pin = ultrasonic sensor GND pin
Raspberry Pi 16pin = ultrasonic sensor Trig pin
Raspberry Pi 18pin = Register 2K pin = ultrasonic sensor Echo pin
Raspberry Pi 20pin = Register 1K pin = ultrasonic sensor Echo pin
Raspberry Pi 39pin = Switch third pin

import RPi.GPIO as GPIO
from time import sleep
import os
import argparse
import cv2
import numpy as np
import sys
import time
from threading import Thread
import importlib.util
import signal

left_switch = 3 #Switch assumed as signal light

#GPIO Setting
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(left_switch,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(right_switch,GPIO.IN,GPIO.PUD_UP)

def distanceInCm(duration): #Function that returns duration as distance in cm
return (duration/2)/29.1

def print_distance(distance): #A function that sets the distanceMsg differently depending on the distance value.
if distance == 0:
distanceMsg = ‘Distance : out of range \r’
else:
distanceMsg = ‘Distance : ‘+str(distance)+’cm’+’ \r’
return distanceMsg

#Set pin number of ultrasonic sensor
TRIG = 23 #TRIG Numbering
ECHO = 24 #ECHO Numbering

MAX_DISTANCE_CM = 300
MAX_DURATION_TIMEOUT = (MAX_DISTANCE_CM * 2 * 29.1)

GPIO.setmode(GPIO.BCM)

GPIO.setup(TRIG, GPIO.OUT) #TRIG Ultrasonic Signal Transmission Pin Designation and Specify Output
GPIO.setup(ECHO, GPIO.IN) #Specify ECHO Ultrasonic Signal Receiving Pin and Specify Output

#Thread to manage webcam
class VideoStream:
“”"Camera object that controls video streaming from the Picamera”"”
def __init__(self, resolution=(640,480), framerate=30):
self.stream = cv2.VideoCapture(0) #Trun on the camera
ret = self.stream.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*’MJPG’))
#frame size init
ret = self.stream.set(3, resolution[0])
ret = self.stream.set(4, resolution[1])

# Read first frame from the stream
(self.grabbed, self.frame) = self.stream.read()

# Variable to control when the camera is stopped
self.stopped = False

def start(self): # Start the thread that reads frames from the video stream
Thread(target=self.update,args=()).start()
return self
def update(self): #Keep looping indefinitely until the thread is stopped
while True:
#If the camera is stopped, stop the thread
if self.stopped:
#Close camera resources
self.stream.release()
return
#Otherwise, grab the next frame from the stream
(self.grabbed, self.frame) = self.stream.read()

def read(self): #Return the most recent frame
return self.frame

def stop(self): #Indicate that the camera and thread should be stopped
self.stopped = True

#Define and parse input arguments
parser = argparse.ArgumentParser()
parser.add_argument(‘–modeldir’, help=’Folder the .tflite file is located in’, required=True)

parser.add_argument(‘–edgetpu’, help=’Use Coral Edge TPU Accelerator to speed up detection’, action=’store_true’)

args = parser.parse_args()

MODEL_NAME = args.modeldir
GRAPH_NAME = args.graph
LABELMAP_NAME = args.labels
min_conf_threshold = float(args.threshold)
resW, resH = args.resolution.split(‘x’)
imW, imH = int(resW), int(resH)
use_TPU = args.edgetpu

#Import TensorFlow libraries
#If tensorflow is not installed, import interpreter from tflite_runtime, else import from regular tensorflow
#If using Coral Edge TPU, import the load_delegate library
pkg = importlib.util.find_spec(‘tensorflow’)
if pkg is None:
from tflite_runtime.interpreter import Interpreter
if use_TPU:
from tflite_runtime.interpreter import load_delegate
else:
from tensorflow.lite.python.interpreter import Interpreter
if use_TPU:
from tensorflow.lite.python.interpreter import load_delegate

#If using Edge TPU, assign filename for Edge TPU model
if use_TPU: #If user has specified the name of the .tflite file, use that name, otherwise use default ‘edgetpu.tflite’
if (GRAPH_NAME == ‘detect.tflite’):
GRAPH_NAME = ‘edgetpu.tflite’

#Get path to current working directory
CWD_PATH = os.getcwd()

#Path to .tflite file, which contains the model that is used for object detection
PATH_TO_CKPT = os.path.join(CWD_PATH,MODEL_NAME,GRAPH_NAME)

#Path to label map file
PATH_TO_LABELS = os.path.join(CWD_PATH,MODEL_NAME,LABELMAP_NAME)

#Load the label map
with open(PATH_TO_LABELS, ‘r’) as f:
labels = [line.strip() for line in f.readlines()]

#Have to do a weird fix for label map if using the COCO “starter model” from
#https://www.tensorflow.org/lite/models/object_detection/overview
#First label is ‘???’, which has to be removed.
if labels[0] == ‘???’:
del(labels[0])

#Load the Tensorflow Lite model.
#If using Edge TPU, use special load_delegate argument
if use_TPU:
interpreter = Interpreter(model_path=PATH_TO_CKPT,
experimental_delegates=[load_delegate('libedgetpu.so.1.0')])
print(PATH_TO_CKPT)
else:
interpreter = Interpreter(model_path=PATH_TO_CKPT)

interpreter.allocate_tensors()

#Get model details
input_details = interpreter.get_input_details()
output_details = interpreter.get_output_details()
height = input_details[0]['shape'][1] width = input_details[0]['shape'][2]

floating_model = (input_details[0]['dtype'] == np.float32)

input_mean = 127.5
input_std = 127.5

#Initialize frame rate calculation
frame_rate_calc = 1
freq = cv2.getTickFrequency()

#Initialize video stream
videostream = VideoStream(resolution=(imW,imH),framerate=30).start()
time.sleep(1)

#for frame1 in camera.capture_continuous(rawCapture, format=”bgr”,use_video_port=True):
while True:
if GPIO.input(left_switch) == 1: #Start timer (for calculating frame rate)
t1 = cv2.getTickCount()

#Grab frame from video stream
frame1 = videostream.read()

#Acquire frame and resize to expected shape [1xHxWx3] frame = frame1.copy()
frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame_resized = cv2.resize(frame_rgb, (width, height))
input_data = np.expand_dims(frame_resized, axis=0)

#Normalize pixel values if using a floating model (i.e. if model is non-quantized)
if floating_model:
input_data = (np.float32(input_data) – input_mean) / input_std

#Perform the actual detection by running the model with the image as input
interpreter.set_tensor(input_details[0]['index'],input_data)
interpreter.invoke()

#Retrieve detection results
boxes = interpreter.get_tensor(output_details[0]['index'])[0] #Bounding box coordinates of detected objects
classes = interpreter.get_tensor(output_details[1]['index'])[0] #Class index of detected objects
scores = interpreter.get_tensor(output_details[2]['index'])[0] #Confidence of detected objects
#num = interpreter.get_tensor(output_details[3]['index'])[0] #Total number of detected objects (inaccurate and not needed)

pulse_start=0
pulse_end=0

GPIO.output(TRIG, False) #Keep the trigger pin off.
time.sleep(2)

GPIO.output(TRIG, True) #Exits 10us pulse.
time.sleep(0.00001) #At Python, this pulse will be almost 100 us.
GPIO.output(TRIG, False)

timeout = time.time()
while GPIO.input(ECHO) == 0:
pulse_start = time.time() #Set the start time when the echo pin is turned on.
if ((pulse_start – timeout)*1000000) >= MAX_DURATION_TIMEOUT:
fail = True
break

timeout = time.time()
while GPIO.input(ECHO) == 1:
pulse_end = time.time() #Set the reflector receiving time when the echo pin is turned off.
if ((pulse_end – pulse_start)*1000000) >= MAX_DURATION_TIMEOUT:
print_distance(0)
fail = True
break

pulse_duration = (pulse_end – pulse_start) * 1000000
pulse_duration=pulse_end-pulse_start #Calculate pulse length
distance = distanceInCm(pulse_duration)
distance=(pulse_duration*34000.0)/2 #The duration is divided in half.
distance = round(distance, 2)
MSG=print_distance(distance)

#Loop over all detections and draw detection box if confidence is above minimum threshold
for i in range(len(scores)):
if ((scores[i] > min_conf_threshold) and (scores[i] <= 1.0)):

#Get bounding box coordinates and draw box
#Interpreter can return coordinates that are outside of image dimensions, need to force them to be within image using max() and min()
ymin = int(max(1,(boxes[i][0] * imH)))
xmin = int(max(1,(boxes[i][1] * imW)))
ymax = int(min(imH,(boxes[i][2] * imH)))
xmax = int(min(imW,(boxes[i][3] * imW)))

cv2.rectangle(frame, (xmin, ymin), (xmax, ymax), (10, 255, 0), 2)
a=(xmax-xmin)/2
b = int(a)
if distance < 16:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 0, 255), 3)
elif distance >=16 and distance < 20:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 150, 255), 3)
else:
frame = cv2.line(frame, (650, 720), (xmin+b, ymax), (0, 255, 0), 3)

#Draw label
object_name = labels[int(classes[i])] #Look up object name from “labels” array using class index
label = ‘%s: %d%%’ % (object_name, int(scores[i]*100)) #Example: ‘person: 72%’
labelSize, baseLine = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.7, 2) #Get font size
label_ymin = max(ymin, labelSize[1] + 10) #Make sure not to draw label too close to top of window
cv2.rectangle(frame, (xmin, label_ymin-labelSize[1]-10), (xmin+labelSize[0], label_ymin+baseLine-10), (255, 255, 255), cv2.FILLED) #Draw white box to put label text in
cv2.putText(frame, label, (xmin, label_ymin-7), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 0), 2) #Draw label text

#Draw distance in corner of frame
cv2.putText(frame, MSG,(30,50),cv2.FONT_HERSHEY_SIMPLEX,1,(255,255,0),2,cv2.LINE_AA)

#All the results have been drawn on the frame, so it’s time to display it.
cv2.imshow(‘Object detector’, frame)

#Calculate framerate
t2 = cv2.getTickCount()
time1 = (t2-t1)/freq
frame_rate_calc= 1/time1

#Press ‘q’ to quit
if cv2.waitKey(1) == ord(‘q’):
break
else: #Exit program if switch off
break
#Clean up
GPIO.cleanup()
cv2.destroyAllWindows()
videostream.stop()

#include <Servo.h>
Servo servo;
// 각 핀 번호 설정
int value=0;
int echo1 = 8;
int trig1 = 9;
int echo2 = 10;
int trig2 = 11;
int red=4;
int green=3;
// 각 핀의 입출력 설정
void setup() {
Serial.begin(9600);
pinMode(trig1, OUTPUT);
pinMode(echo1, INPUT);
pinMode(trig2, OUTPUT);
pinMode(echo2, INPUT);
pinMode(red,OUTPUT);
pinMode(green,OUTPUT);
servo.attach(7);
}

void loop() {
// 1번 초음파센서에서 초음파 발사 후 초음파가 다시 돌아오는데 걸리는 시간 저장
digitalWrite(trig1, LOW);
digitalWrite(echo1, LOW);
delayMicroseconds(2);
digitalWrite(trig1, HIGH);
delayMicroseconds(10);
digitalWrite(trig1, LOW);
unsigned long duration1 = pulseIn(echo1, HIGH);
// 2번 초음파센서에서 초음파 발사 후 초음파가 다시 돌아오는데 걸리는 시간 저장
digitalWrite(trig2, LOW);
digitalWrite(echo2, LOW);
delayMicroseconds(2);
digitalWrite(trig2, HIGH);
delayMicroseconds(10);
digitalWrite(trig2, LOW);
unsigned long duration2 = pulseIn(echo2, HIGH);
// 초음파를 발산한 후 다시 돌아온 시간을 가지고 거리 측정
float distance1 = ((float)(340 * duration1) / 10000) / 2;
float distance2 = ((float)(340 * duration2) / 10000) / 2;

// 1번 초음파센서로 측정한 거리가 13cm미만일 경우 (쓰레기통에 근접하는 물체가 감지 될 경우)
if(distance1<13) // 쓰레기통의 문을 연다
{
for(;value>=0;value -=2)
{
servo.write(value);
delay(10);
}
delay(3000); // 문이 열린 후 3초의 지연시간을 준다
}else // 아닐 경우
{
for(;value<=100;value +=2) // 쓰레기통의 문이 닫힌다
{
servo.write(value);
delay(10);
}
}
// 2번 초음파센서로 측정한 거리가 9.5cm초과일 경우(쓰레기통 안의 내용물이 다 찼을 경우)
if(distance2<9.5)
{
// LED 빨간불 ON
digitalWrite(green,HIGH);
digitalWrite(red,LOW);
}
else // 아닐 경우
{
// LED 초록불 ON
digitalWrite(green,LOW);
digitalWrite(red,HIGH);
}
// 측정된 거리 값을 시리얼 모니터에 출력
Serial.print(“1 : “);
Serial.print(distance1);
Serial.print(“cm “);
Serial.print(“2 : “);
Serial.print(distance2);
Serial.println(“cm”);
delay(100);
}

from sense_hat import SenseHat
import time
import imageprocessing_03 as stair
import MotorController_03 as motor
import cv2
import os
import RPi.GPIO as GPIO

## MOTOR config setting ##

# 모터 상태
STOP = 0
FORWARD = 1
BACKWORD = 2

# 모터 채널
CH1 = 0
CH2 = 1

# PIN 입출력 설정
OUTPUT = 1
INPUT = 0

# PIN 설정
HIGH = 1
LOW = 0

# 실제 핀 정의
#PWM PIN
ENA = 26 #37 pin
ENB = 0 #27 pin

#GPIO PIN
IN1 = 19 #35 pin
IN2 = 13 #33 pin
IN3 = 6 #31 pin
IN4 = 5 #29 pin
SW1 = 17 #13 pin
SW2 = 27 #15 pin
SW3 = 16
SW4 = 20

# 핀 설정 함수
def setPinConfig(EN, INA, INB):
GPIO.setup(EN, GPIO.OUT)
GPIO.setup(INA, GPIO.OUT)
GPIO.setup(INB, GPIO.OUT)
# 100khz 로 PWM 동작 시킴
pwm = GPIO.PWM(EN, 100)
# 우선 PWM 멈춤.
pwm.start(0)
return pwm

# 스위치 핀 함수
def setPinSwitch(SW1, SW2, SW3, SW4) :
GPIO.setwarnings(False)
GPIO.setup(SW1,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW2,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW3,GPIO.IN,GPIO.PUD_UP)
GPIO.setup(SW4,GPIO.IN,GPIO.PUD_UP)

# 모터 제어 함수
def setMotorContorl(pwm, INA, INB, speed, stat):

#모터 속도 제어 PWM
pwm.ChangeDutyCycle(speed)

if stat == FORWARD:
GPIO.output(INA, HIGH)
GPIO.output(INB, LOW)

#뒤로
elif stat == BACKWORD:
GPIO.output(INA, LOW)
GPIO.output(INB, HIGH)

#정지
elif stat == STOP:
GPIO.output(INA, LOW)
GPIO.output(INB, LOW)
# 모터 제어함수 간단하게 사용하기 위해 한번더 래핑(감쌈)
def setMotor(ch, speed, stat):
if ch == CH1:
#pwmA는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmA, IN1, IN2, speed, stat)
else:
#pwmB는 핀 설정 후 pwm 핸들을 리턴 받은 값이다.
setMotorContorl(pwmB, IN3, IN4, speed, stat)

def RC():
if GPIO.input(SW1) == 0 : #forward

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“forward”)

elif GPIO.input(SW2) == 0 : #back

SPEED = 12 #LEFT MOTOR
SPEED1 = 12
setMotor(CH1, SPEED, BACKWORD)
setMotor(CH2, SPEED1, FORWARD)
print(“back”)

elif GPIO.input(SW3) == 0 : #LEFT
SPEED = 1 #LEFT MOTOR
SPEED1 = 15
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“here”)
print(“left”)

elif GPIO.input(SW4) == 0 : #right
SPEED = 15 #LEFT MOTOR
SPEED1 = 1
setMotor(CH1, SPEED, FORWARD)
setMotor(CH2, SPEED1, BACKWORD)
print(“right”)

else :
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
print(“stop”)

###########################

def stair_detection():
while True:
print(‘in rc’)
RC() #..
i = 0
while i < 50 :
sense.get_orientation_degrees()['pitch'] i = i + 1

if stair_threshold <= sense.get_orientation_degrees()['pitch']:
print(“up stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
os.system(“sudo dd if=/dev/video0 of=image.png bs=11M count=1″)

return stair.stair_detection()

elif sense.get_orientation_degrees()['pitch'] <= -stair_threshold :
print(“down stair”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)
os.system(“sudo service uv4l_raspicam restart”)
# down stair sound

return ‘down’
if __name__ == “__main__” :
os.system(“sudo service uv4l_raspicam restart”)
sense = SenseHat()
stair_threshold = 15 # …
roll_threshold = 10 #

# GPIO 모드 설정
GPIO.setmode(GPIO.BCM)

#모터 핀 설정
#핀 설정후 PWM 핸들 얻어옴
pwmA = setPinConfig(ENA, IN1, IN2)
pwmB = setPinConfig(ENB, IN3, IN4)
setPinSwitch(SW1, SW2, SW3, SW4)

SPEED = 12
SPEED1 = 12

while True:
print(” ##### program start ##### “)

detection = stair_detection()

if detection == True: #up stair

print(‘up stair’)
#up stair sound mp3
while sense.get_orientation_degrees()['pitch'] > 3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 10, BACKWORD)
print(‘rigth move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 13, BACKWORD)
print(‘straight’)

elif detection == ‘down’ :

print(‘down stair’)
while sense.get_orientation_degrees()['pitch'] < -3 :

if sense.get_orientation_degrees()['roll'] > roll_threshold:
setMotor(CH1, 0, STOP)
setMotor(CH2, 13, BACKWORD)
print(‘right move’)

elif sense.get_orientation_degrees()['roll'] < -roll_threshold:
setMotor(CH1, 13, FORWARD)
setMotor(CH2, 0, STOP)
print(‘left move’)
elif -roll_threshold < sense.get_orientation_degrees()['roll'] < roll_threshold:
setMotor(CH1, 10, FORWARD)
setMotor(CH2, 10, BACKWORD)
print(‘straight’)

elif detection == False : #wall or 1 stair

print(“wall”)
setMotor(CH1, 0, STOP)
setMotor(CH2, 0, STOP)

GPIO.cleanup()

//헤더파일

//스테핑모터 정보. 모터 스펙에 맞게 설정
const int stepsPerRevolution = 2048; //28BYJ-48는 1회전당 2048스텝

//핀번호
//블루투스 설정
SoftwareSerial BTSerial(BT_RXD, BT_TXD); // change this to fit the number of steps per revolution

//data 읽기 시작, 종료 상태 변수
bool isStart = false; // 읽기 시작
bool isEnd = false; // 읽기 종료
String resultStr = “”; // 최종 Data를 저장할 문자열

//불루투스에서 받은 값을 나눠서 넣을 값.

//서브모터 설정
Servo servo; // Servo 클래스로 servo 객체 생성
int angle = 0; // Servo 모터의 각도 (0~180)
int stAngle = 82; //Servo 모터 직진용 각도, 82도가 직진, 사용하기 전에 기기마다 맞는 설정값을 입력해줘야 함
double ccAngle = 0; // 원형 운동시 서브모터 각도, 어플에서 넘어온 값으로 변경됨
int tnAngle = 0; // 혼합 운동시 서브모터 각도, 어플에서 넘어온 값으로 변경됨

//바퀴 데이터
int wheelRadius = 3; //바퀴 반지름
float wheelCircum = wheelRadius * 2 * 3.141592; // 바퀴 원주
//직선 운동시 필요 데이터를 계산하기 위한 변수
float stTurnNum = 0; //회전수
float stStepPerRevolution = 0; //step per revolution
float stSpeedNeed = 0; //회전속도(rpm)을 구하기 위해 필요한 변수
float stStepSpeed = 0; //회전속도(rpm)

//원형 운동시 필요 데이터를 계산하기 위한 변수
float ccTurnNum = 0; //회전수
float ccDis = 0; //회전 이동거리
float ccStepPerRevolution = 0; //step per revolution
float ccSpeedNeed = 0; //회전속도(rpm)을 구하기 위해 필요한 변수
float ccStepSpeed = 0; //회전속도(rpm)

//직선 운동3에 필요한 데이터를 계산하기 위한 변수
int stTurnNum3 = 0; //회전수
int stStepPerRevolution3 = 0; //step per revolution
int stSpeedNeed3 = 0; //회전속도(rpm)을 구하기 위해 필요한 변수
int stStepSpeed3 = 0; //회전속도(rpm)

//몸체 데이터
float bodylength = 10; // 몸체 세로 길이. 회전각을 구할 때에 필요

//,를 찾아내기 위한 변수

//회전에 필요한 데이터
int turnAngle = 0;
float turnData = 0;
void setup() {
//통신 설정
Serial.begin(9600); //컴퓨터와의 통신속도 설정
BTSerial.begin(9600); //블루투스와의 통신속도 설정

//Servo 모터 핀 설정
servo.attach(servoPin); //servoPin 데이터는 위에 표기
}

void loop() {

if (BTSerial.available()) {
while (1) {
char val = BTSerial.read(); //블루투스 데이터를 받아와 val변수에 삽입

//데이터 읽기 시작
if (val == ‘<’) {
Serial.println(“start”);
resultStr = “”; // 최종 Data 문자열 초기화
isStart = true;
isEnd = false;
continue; // while문으로 바로 이동
}
// 데이터 읽기 종료
else if (val == ‘>’) {
Serial.println(“end”);
isEnd = true;
break;
}
// 중간 데이터값을 최종 Data 문자열에 추가
else {
resultStr = String(resultStr + val);
}
}

//시작 부호와 종료 부호가 들어왔다면
if (isStart == true && isEnd == true) {
Serial.print(“전송된 데이터 : “);
Serial.println(resultStr);

//컴마 기호 파악
//문자열 나누기

//다음 불루투스 값을 받아들이기 위한 변수 초기화
isStart = false;
isEnd = false;

//직선 운동
if (sep == 2) {
//Serial.println(“직선 운동 모드로 설정되었습니다.”);

//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

//직선운동 스텝모터 작동에 필요한 값 계산
stTurnNum = (float)dis1 / wheelCircum; //회전수
stSpeedNeed = stTurnNum * 60; //계산과정 추후에 정리
stStepPerRevolution = (float)stTurnNum * stepsPerRevolution; //모터 한바퀴가 2048step이기 때문에 stepsPerRevolution을 곱함
stStepSpeed = (float)stSpeedNeed / tim1; //회전속도(rpm)

//직진모드
//후진모드
//왕복모드
else if (mod == 2) {
//Serial.println(“직선운동의 왕복모드가 시작되었습니다.”);
goForward(stStepSpeed, stStepPerRevolution);
goBackward(stStepSpeed, stStepPerRevolution);
}
}

//원형 운동
else if (sep == 3) {

//Serial.println(“원형 운동 모드로 설정되었습니다.”);

// 서보모터 회전각도 계산
// 계산과정은 나중에 따로 정리하겠습니다.
turnData = (float)rad2 / bodylength;
ccAngle = 180 – 2 * atan(turnData);

//서보모터 회전
servo.write(ccAngle); //서보모터 각도 설정
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

//스텝모터 관련 계산
ccDis = (float)0.01756 * ang2 * rad2; //회전 이동거리
ccTurnNum = (float)ccDis / wheelCircum; //회전수. 회전수는 이동거리 나누기 바퀴 원주
ccStepPerRevolution = (float)ccTurnNum * stepsPerRevolution; //step per revolution
ccSpeedNeed = ccTurnNum * 60; //회전속도(rpm)을 구하기 위해 필요한 변수
ccStepSpeed = (float)ccSpeedNeed / tim2; //회전속도(rpm)

//직진모드
//후진모드
//왕복모드
else if (mod == 2) {
//Serial.println(“원형운동의 왕복모드가 시작되었습니다.”);
goForward(stStepSpeed, stStepPerRevolution);
goBackward(stStepSpeed, stStepPerRevolution);
}
}

//회전운동 모드
else if (sep == 4) {

//직선운동1 스텝모터 작동에 필요한 값 계산
stTurnNum = (float)dis1 / wheelCircum; //회전수
stSpeedNeed = stTurnNum * 60; //계산과정 추후에 정리
stStepPerRevolution = (float)stTurnNum * stepsPerRevolution; //모터 한바퀴가 2048step이기 때문에 stepsPerRevolution을 곱함
stStepSpeed = (float)stSpeedNeed / tim1; //회전속도(rpm)

//스텝모터 회전 관련 계산
ccDis = (float)0.01756 * ang2 * rad2; //회전 이동거리
ccTurnNum = (float)ccDis / wheelCircum; //회전수. 회전수는 이동거리 나누기 바퀴 원주
ccStepPerRevolution = (float)ccTurnNum * stepsPerRevolution; //step per revolution
ccSpeedNeed = ccTurnNum * 60; /회전속도(rpm)을 구하기 위해 필요한 변수
ccStepSpeed = (float)ccSpeedNeed / tim2; //회전속도(rpm)

//직선운동3 스텝모터 작동에 필요한 값 계산
stTurnNum3 = (float)dis3 / wheelCircum; //회전수
stSpeedNeed3 = stTurnNum * 60; //계산과정 추후에 정리
stStepPerRevolution3 = (float)stTurnNum * stepsPerRevolution; //모터 한바퀴가 2048step이기 때문에 stepsPerRevolution을 곱함
stStepSpeed3 = (float)stSpeedNeed / tim3; //회전속도(rpm)

//직진모드
if (mod == 0) {
//Serial.println(“회전 운동 직진모드로 설정되었습니다.”);

//직진1
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

//직선운동에서 스텝모터 움직임
goForward(stStepSpeed, stStepPerRevolution);

//회전
// 계산과정은 나중에 따로 정리하겠습니다.
turnData = (float)rad2 / bodylength;
ccAngle = 180 – 2 * atan(turnData);

//서보모터 회전
for (int i = 0; i < ccAngle; i++) {
turnAngle += 1;
servo.write(turnAngle); //서보모터 각도 설정
}

//스텝모터 관련 계산
ccDis = (float)0.01756 * ang2 * rad2; //회전 이동거리
ccTurnNum = (float)ccDis / wheelCircum; //회전수. 회전수는 이동거리 나누기 바퀴 원주
ccStepPerRevolution = (float)ccTurnNum * stepsPerRevolution; //step per revolution
ccSpeedNeed = ccTurnNum * 60; //전속도(rpm)을 구하기 위해 필요한 변수
ccStepSpeed = (float)ccSpeedNeed / tim2; //회전속도(rpm)
goForward(ccStepSpeed, ccStepPerRevolution);

// 직진3
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

goForward(stStepSpeed, stStepPerRevolution);
}

else if (mod == 2) {
//Serial.println(“회전 운동 왕복모드 직진부로 설정되었습니다.”);

//직진1
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

//직선운동에서 스텝모터 움직임
goForward(stStepSpeed, stStepPerRevolution);

//회전
// 계산과정은 나중에 따로 정리하겠습니다.
turnData = (float)rad2 / bodylength;
ccAngle = 180 – 2 * atan(turnData);

//서보모터 회전
for (int i = 0; i < ccAngle; i++) {
turnAngle += 1;
servo.write(turnAngle); //서보모터 각도 설정
}

//스텝모터 관련 계산
ccDis = (float)0.01756 * ang2 * rad2; //회전 이동거리
ccTurnNum = (float)ccDis / wheelCircum;  //회전수. 회전수는 이동거리 나누기 바퀴 원주
ccStepPerRevolution = (float)ccTurnNum * stepsPerRevolution; //step per revolution
ccSpeedNeed = ccTurnNum * 60;  //회전속도(rpm)을 구하기 위해 필요한 변수
ccStepSpeed = (float)ccSpeedNeed / tim2;  //회전속도(rpm)

goForward(ccStepSpeed, ccStepPerRevolution);

// 직진3
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500);  //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

goForward(stStepSpeed, stStepPerRevolution);

//Serial.println(“회전 운동 왕복모드 후진부로 설정되었습니다.”);
//직진1
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

//직선운동에서 스텝모터 움직임
goBackward(stStepSpeed, stStepPerRevolution);

//회전
// 계산과정은 나중에 따로 정리하겠습니다.
turnData = (float)rad2 / bodylength;
ccAngle = 180 – 2 * atan(turnData);

//서보모터 회전
for (int i = ccAngle; i > 0; i–) {
turnAngle -= 1;
}
servo.write(turnAngle); //서보모터 각도 설정

//스텝모터 관련 계산
ccDis = (float)0.01756 * ang2 * rad2; //회전 이동거리
ccTurnNum = (float)ccDis / wheelCircum; //회전수. 회전수는 이동거리 나누기 바퀴 원주
ccStepPerRevolution = (float)ccTurnNum * stepsPerRevolution; //step per revolution
ccSpeedNeed = ccTurnNum * 60; //회전속도(rpm)을 구하기 위해 필요한 변수
ccStepSpeed = (float)ccSpeedNeed / tim2; //회전속도(rpm)

goBackward(stStepSpeed, stStepPerRevolution);

// 직진3
//직선운동에서 서브모터 각도 수정
servo.write(stAngle); //변수 값으로 고정했습니다. 장치 개발 후 수정이 필요합니다.
delay(500); //서브모터가 돌고 바로 스텝모터가 도는 것을 방지하기 위해 잠깐의 텀을 둡니다.

goBackward(stStepSpeed, stStepPerRevolution);
}

}
//변수초기화 혹시 몰라서 넣어봅니다.
turnAngle = 0;
}
}
void goForward(float a, int b) {
rightStepper.setSpeed(a);
leftStepper.setSpeed(a);
for (int i = 0; i < b; i++) {
rightStepper.step(1);
leftStepper.step(-1);
}
}

void goBackward(float c, int d) {
rightStepper.setSpeed(c);
leftStepper.setSpeed(c);
for (int i = 0; i < d; i++) {
rightStepper.step(-1);
leftStepper.step(1);
}
}

① BT_connect -
블루투스를 이용하여 개발 어플과 Dollying(기기)을 연동

using TechTweaking.Bluetooth; // BT 제어 모듈
public class BT_connect : MonoBehaviour
{
private BluetoothDevice device;

BT연결 설정
private void Awake()
{
BluetoothAdapter.enableBluetooth(); //Force Enabling Bluetooth
device = new BluetoothDevice();
device.Name = “BT1″; // 내 블루투스 이름
}
public void connect()
{
device.connect();
}

public void disconnect()
{
device.close();
}
}
② 어플 페이지 설정

void Start()
{
ContextChanger(“1Page1″); // 앱 실행시 로고 출력

// 2초 후 2Page2 내용 출력
Invoke(“ShowFirstContext”, 2f);
}
public void ShowFirstContext()
{
ContextChanger(“2Page2″);
}
//콘텐츠 활성화 및 비활성화 처리
/// <param name=”conName”>콘텐츠 활성화 시킬 게임오브젝트 이름</param>
public void ContextChanger(string conName)
{
for (int i = 0; i < contextArr.Length; i++)
{
if (contextArr[i].name == conName)
{
if (i != 5) prevContextName = conName;

contextArr[i].SetActive(true);

if (i < 5) menu = i;
switch (i)
{
case 2:
mode = dirType1;
break;
case 3:
mode = dirType2;
break;
case 4:
mode = dirType3;
break;
default:
break;
}
}
else
{
contextArr[i].SetActive(false);
}
}
}
// 직전 설정 화면으로 이동하는 함수
public void BackToModify()
{
ContextChanger(prevContextName);
}
③ 블루투스로 데이터 전송하기
public void SendDataToBt()
{
if (device != null)
{
string distance1;
string time1;
string distance2;
string time2;
string angle2;
//혼합이동을 위해 나누어 처리
if (menu == 4)
{
distance1 = inputField[7].text; //어플을 통해 사용자로 부터 입력받은 데이터를 변수값에 넣어줌
time1 = inputField[8].text;
distance2 = inputField[9].text;
time2 = inputField[10].text;
angle2 = inputField[11].text;
}
else
{
distance1 = inputField[0].text;
time1 = inputField[1].text;
distance2 = inputField[2].text;
time2 = inputField[3].text;
angle2 = inputField[4].text;
}

string distance3 = inputField[5].text;
string time3 = inputField[6].text;
string menu1 = menu.ToString();
string mode1 = mode.ToString();

//아두이노가 값을 읽어처리하는 형식과 동일하게 구성시켜 데이터를 보내줌
string resultStr = “<” + menu1 + “,” + distance1 + “,” + time1 + “,” + distance2 + “,” + time2 + “,” + angle2 + “,” + distance3 + “,” + time3 + “,” + mode1 + “>”;
device.send(System.Text.Encoding.ASCII.GetBytes(resultStr));
Debug.Log(resultStr);
//t.GetComponent<Text>().text = resultStr;

// 타이머 설정(마지막 페이지(잔여시간 보여주기 기능)를 위해)
timer = 0;
switch (menu)
{
case 2:
timer = int.Parse(time1);
if (mode == 2)
{
timer *= 2;
}
break;
case 3:
timer = int.Parse(time2);
if (mode == 2)
{
timer *= 2;
}
break;
case 4:
timer = int.Parse(time1) + int.Parse(time2) + int.Parse(time3);
if (mode == 2)
{
timer *= 2;
}
break;
default:
break;
}

maxTimer = timer;
timerGuage.fillAmount = 1f;
// 타이머 UI 실행 및 타이머 감소
StartCoroutine(TimerCoroutine());
}

const int a = 8;
const int b = A0;
const int c = A1;
int x;
//a는 워터펌프 b,c는 센서
//a는 8번핀에 연결 b,c는 각각 A0,A1에 연결

void setup()
{
pinMode(a, OUTPUT);
Serial.begin(9600);
x=0;
}

void loop()
{
int lax1 = analogRead(b);
int lax2 = analogRead(c);
if(lax1<500||lax2<500)
{
digitalWrite(a, HIGH);
Serial.print(lax1);
Serial.print(” “);
Serial.println(lax2);
x=1;
delay(50);
}
else
{
if(x==1)
{
delay(5000); // 센서가 인식범위를 벗어났을때 워터펌프가 일정시간 더 작동하기 위한 시간 1000=1초
x=0;
}
else if(x==0)
{
digitalWrite(a, LOW);
Serial.print(lax1);
Serial.print(” “);
Serial.println(lax2);
}
delay(50);
}
}
// Serial 코딩은 문제를 확인하기 위한 코딩이므로 정상적으로 작동한다 생각시 삭제해도 상관없다
// 센서 인식범위를 늘릴 생각이면 if조건문에 500을 조정하면된다. 숫자가 높아질수록 민감해지고 최대 1023까지 조정 할수 있다

#include <SoftwareSerial.h>
int Tx=2; //블루투스 전송
int Rx=3; //블루투스 수신
SoftwareSerial btSerial(Tx, Rx);
void setup() {
Serial.begin(9600);
btSerial.begin(9600);
//오르막길 센서(distance4)
pinMode(9, OUTPUT);
pinMode(8, INPUT);
//발바닥 센서(distance1)
pinMode(7,OUTPUT);
pinMode(6,INPUT);
//전방 센서(distance2)
pinMode(5, OUTPUT);
pinMode(4, INPUT);
//내리막길 센서(distance3)
pinMode(13,OUTPUT);
pinMode(12, INPUT);
}
void loop() {
//발바닥 센서 측정 시작한 후, 거리값 계산
digitalWrite(7, LOW);
digitalWrite(6, LOW);
delayMicroseconds(2);
digitalWrite(7,HIGH);
delayMicroseconds(10);
digitalWrite(7,LOW);
unsigned long duration1 = pulseIn(6, HIGH);
float distance1 = ((float)(340L*duration1)/10000)/2.0;

//발바닥 센서에서 측정한 값이 3.8cm 이하일 때, 나머지 센서(전방 센서, 오르막길 센서, 내리막길 센서) 측정 시작한 후 거리값 계산
if(distance1<3.8f){
Serial.println(“Under 10cm”);
digitalWrite(9, LOW);
digitalWrite(8, LOW);
delayMicroseconds(2);
digitalWrite(9, HIGH);
delayMicroseconds(10);
digitalWrite(8,LOW);
unsigned long duration4 = pulseIn(8, HIGH);
float distance4 = ((float)(340L*duration4)/10000)/2.0;

digitalWrite(5, LOW);
digitalWrite(4, LOW);
delayMicroseconds(2);
digitalWrite(5, HIGH);
delayMicroseconds(10);
digitalWrite(5,LOW);
unsigned long duration2 = pulseIn(4, HIGH);
float distance2 = ((float)(340L*duration2)/10000)/2.0;

digitalWrite(13, LOW);
digitalWrite(12, LOW);
delayMicroseconds(2);
digitalWrite(13, HIGH);
delayMicroseconds(10);
digitalWrite(13,LOW);
unsigned long duration3 = pulseIn(12, HIGH);
float distance3 = ((float)(340L*duration3)/10000)/2.0;
Serial.println(distance2);
Serial.println(distance3);
Serial.println(distance4);
//센서가 측정 가능한 거리값을 과도하게 벗어나는 오류값을 받지 않기 위해 3500cm 이하의 값만 안드로이드 스튜디오로 전송
if (distance2<3500&&distance3<3500&&distance4<3500){
btSerial.print(distance2);
btSerial.print(‘,’);
btSerial.print(distance3);
btSerial.print(‘,’);
btSerial.print(distance4);
btSerial.println();
}
}
else{

}
delay(1000);
}

Mainactivity.java
package com.example.sinbal;
import android.app.Activity;
import android.content.Intent;
import android.widget.TextView;
import android.widget.Toast;
import app.akexorcist.bluetotohspp.library.BluetoothSPP;
import app.akexorcist.bluetotohspp.library.BluetoothState;
import app.akexorcist.bluetotohspp.library.DeviceList;
import android.media.MediaPlayer;
import android.bluetooth.BluetoothAdapter;
import androidx.appcompat.app.AppCompatActivity;
import android.os.Bundle;
import android.view.View;
import android.widget.Button;
public class MainActivity extends AppCompatActivity {
private BluetoothSPP bt;
int size;
boolean blockDetected = false;
boolean wallDetected = false;
boolean downhillDetected = false;
@Override
protected void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_main);
Intent intent = getIntent(); //변수 받아오기 추가 부분
size = intent.getIntExtra(“size”,0);
bt = new BluetoothSPP(this); //Initializing
if (!bt.isBluetoothAvailable()) { //블루투스 사용 불가
Toast.makeText(getApplicationContext()
, “Bluetooth is not available”
, Toast.LENGTH_SHORT).show();
finish();
}
bt.setOnDataReceivedListener(new BluetoothSPP.OnDataReceivedListener() {
TextView distance22 = findViewById(R.id.distance2); //텍스트뷰를 통해 초음파 센서 값 받아오기
TextView distance33 = findViewById(R.id.distance3);
TextView distance44 = findViewById(R.id.distance4);
double finalSize = MainActivity.this.size/10*2.5*3; // final size를 사용자의 보폭 기준 3걸음으로 설정
public void onDataReceived(byte[] data, String message) { //데이터 수신용 코드 추가
String[] array = message.split(“,”);
distance22.setText(array[0].concat(“cm”));
distance33.setText(array[1].concat(“cm”));
distance44.setText(array[2].concat(“cm”));
double distance2 = Double.parseDouble(array[0]); //초음파센서값 3개 array 형식으로 안드로이드 스튜디오에 받아오기
double distance3 = Double.parseDouble(array[1]);
double distance4 = Double.parseDouble(array[2]);
double hypotenuse = distance2/0.93969; //오르막센서 각도를 20도로 설정. 밑변 길이를 cos20으로 나눈 값, 대각선 길이
//벽 & 오르막
if(distance4<400){
if(distance4>hypotenuse){ //오르막 센서값과 전방센서값이 직각삼각형 형성하지 않을 경우 오르막으로 인식
//오르막
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.uphill);
mp.start();
}
else{ // 오르막 센서값과 전방센서값이 직각삼각형 형성할 경우 벽으로 인식
//벽
if(distance2 < finalSize){
if(!wallDetected) { //벽 중복 알람 방지
wallDetected = true;
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.wall);
mp.start();
}
if(distance2 < 15) { //근접 경보
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.close);
mp.start();
}
}
else {
wallDetected = false;
}
}
}
//장애물
else{ // 오르막 센서가 감지되지 않을 경우, 전방센서값은 측정되는 경우.
if(distance2< finalSize){
if(!blockDetected) { //장애물 중복 알람 방지
blockDetected = true;
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.block);
mp.start();
}
if(distance2<15) { //근접 경보
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.close);
mp.start();
}
}
else {
blockDetected = false;
}
}
//내리막센서가 일정 값 이상이 나올 경우
//내리막
if(distance3>100) {
if (!downhillDetected) {
downhillDetected = true; //내리막길 중복 알람 방지
final MediaPlayer mp = MediaPlayer.create(MainActivity.this, R.raw.downhill);
// mp.start();
}
else {
downhillDetected = false;
}
}
});
bt.setBluetoothConnectionListener(new BluetoothSPP.BluetoothConnectionListener() { //연결됐을 때
public void onDeviceConnected(String name, String address) {
Toast.makeText(getApplicationContext()
, “Connected to ” + name + “\n” + address
, Toast.LENGTH_SHORT).show();
}
public void onDeviceDisconnected() { //연결해제
Toast.makeText(getApplicationContext()
, “Connection lost”, Toast.LENGTH_SHORT).show();
}
public void onDeviceConnectionFailed() { //연결실패
Toast.makeText(getApplicationContext()
, “Unable to connect”, Toast.LENGTH_SHORT).show();
}
});
Button btnConnect = findViewById(R.id.btnConnect); //연결시도
btnConnect.setOnClickListener(new View.OnClickListener() {
public void onClick(View v) {
if (bt.getServiceState() == BluetoothState.STATE_CONNECTED) {
bt.disconnect();
} else {
Intent intent = new Intent(getApplicationContext(), DeviceList.class);
startActivityForResult(intent, BluetoothState.REQUEST_CONNECT_DEVICE);
}
}
});
}
public void onDestroy() {
super.onDestroy();
bt.stopService(); //블루투스 중지
}
public void onStart() {
super.onStart();
if (!bt.isBluetoothEnabled()) { //
Intent intent = new Intent(BluetoothAdapter.ACTION_REQUEST_ENABLE);
startActivityForResult(intent, BluetoothState.REQUEST_ENABLE_BT);
} else {
if (!bt.isServiceAvailable()) {
bt.setupService();
bt.startService(BluetoothState.DEVICE_OTHER); //DEVICE_ANDROID는 안드로이드 기기 끼리
setup();
}
}
}
public void setup() {
Button btnSend = findViewById(R.id.btnSend); //데이터 전송
btnSend.setOnClickListener(new View.OnClickListener() {
public void onClick(View v) {
bt.send(“Text”, true);
}
});
}
public void onActivityResult(int requestCode, int resultCode, Intent data) {
if (requestCode == BluetoothState.REQUEST_CONNECT_DEVICE) {
if (resultCode == Activity.RESULT_OK)
bt.connect(data);
} else if (requestCode == BluetoothState.REQUEST_ENABLE_BT) {
if (resultCode == Activity.RESULT_OK) {
bt.setupService();
bt.startService(BluetoothState.DEVICE_OTHER);
setup();
} else {
Toast.makeText(getApplicationContext()
, “Bluetooth was not enabled.”
, Toast.LENGTH_SHORT).show();
finish();
}
}
super.onActivityResult(requestCode, resultCode, data);
} }

Initial.java
package com.example.sinbal;
import android.content.Intent;
import android.content.SharedPreferences;
import android.os.Bundle;
import android.view.View;
import android.widget.Button;
import android.widget.CheckBox;
import android.widget.EditText;
import androidx.appcompat.app.AppCompatActivity;
public class initial extends AppCompatActivity {
Button button;
EditText userSize;
CheckBox checkSize;
SharedPreferences UserInfo;
SharedPreferences.Editor editor;
public static final int sub = 1001; /*다른 액티비티를 띄우기 위한 요청코드(상수)*/
@Override
public void onCreate(Bundle savedInstanceState) {
super.onCreate(savedInstanceState);
setContentView(R.layout.activity_initial);
button = (Button) findViewById(R.id.alert);
userSize = (EditText) findViewById(R.id.inputSize);
checkSize = (CheckBox) findViewById(R.id.checkSize);
checkSize.setChecked(true);
UserInfo = getSharedPreferences(“UserInfo”, 0);
editor = UserInfo.edit();
//사용자에게 발사이즈 입력받는 변수 설정
final int inputSizeValue = UserInfo.getInt(“inputSize”, 0);
if (inputSizeValue > 0) {
userSize.setText(Integer.toString(inputSizeValue));
}
Button button = findViewById(R.id.start); /*페이지 전환버튼*/
button.setOnClickListener(new View.OnClickListener() {
@Override
public void onClick(View v) {
Intent intent = new Intent(getApplicationContext(), MainActivity.class); //변수 받아오기 추가 부분
// Intent intent = new Intent(this, MainActivity.class);
String SizeValue = userSize.getText().toString();
if (!SizeValue.isEmpty()) {//Empty 아닐 때만 실행
int inputSizeValue = Integer.parseInt(SizeValue);
intent.putExtra(“size”, inputSizeValue);
startActivity(intent);//액티비티 띄우기
}
}
});
}
public void onPause() {
super.onPause();
if (checkSize.isChecked()) {
String sSize = userSize.getText().toString();
if (!sSize.isEmpty()) {
editor.putInt(“inputSize”, Integer.parseInt(sSize));
}
} else {
editor.putInt(“inputSize”, 0);
}
editor.commit();
}
public void onStart(){
super.onStart();
button.setOnClickListener(new View.OnClickListener(){
@Override
public void onClick(View v) {
Intent intent = new Intent(initial.this, PopupActivity.class);
startActivity(intent);
}
});
}
}

int selection = 0; //변수지정
#include “U8glib.h” //OLED 출력을 위한 라이브러리 호출
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0|U8G_I2C_OPT_NO_ACK|U8G_I2C_OPT_FAST); //OLED 객체 지정
const uint8_t temp[] PROGMEM = {0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, 0×00, (생략)}; //OLED 디스플레이에 내보낼 이미지
#include <AM2320_asukiaaa.h //온습도센서 라이브러리 호출>
AM2320_asukiaaa mySensor; //온습도센서 객체 지정
int temperature, humidity; //온도, 습도값 변수 지정
void setup() { //셋업함수 시작
{Serial.begin(115200); //시리얼통신 시작
while(!Serial); //시리얼통신 개발 실패시 대기할 것
Wire.begin(); //Wire 통신 시작
mySensor.setWire(&Wire); //온습도센서 통신 시작
u8g.setFont(u8g_font_fub14); //OLED 디스플레이 폰트 지정
u8g.setColorIndex(1); //OLED 디스플레이 글자색 지정
pinMode(2, OUTPUT); //LED 출력핀 지정
pinMode(3, OUTPUT);
pinMode(4, OUTPUT);
pinMode(5, INPUT);} //버튼 입력핀 지정
void loop() { //루프함수 시작
Serial.println(selection); //시리얼 통신을 통해 selection 값을 출력
if (digitalRead(5)==1) {selection++; delay(150);} //만약 버튼이 눌러진다면 selection 값을 1만큼 증가
switch (selection) { //스위치문 시작
case 0: //selection이 0일 경우에
u8g.firstPage(); do {draw0();} while( u8g.nextPage() ); //OLED를 통해 첫번째 이미지를 출력
break; //스위치문 나가기
case 1: //selection이 1일 경우에
digitalWrite(2, 1); //첫번째 LED의 불을 켜고
u8g.firstPage(); do {draw1();} while( u8g.nextPage() ); //OLED를 통해 두번째 이미지를 출력
break; //스위치문 나가기
‘case 2: //selection이 2일 경우에
digitalWrite(2, 0); digitalWrite(3, 1); //첫번째 LED의 불을 끄고 두 번째 LED의 불을 켜고
u8g.firstPage(); do {draw2();} while( u8g.nextPage() ); //OLED를 통해 세번째 이미지를 출력
break; //스위치문 나가기
case 3: //selection이 3일 경우에
digitalWrite(3, 0); digitalWrite(4, 1); //두번째 LED의 불을 끄고 세번째 LED의 불을 켜고
u8g.firstPage(); do {draw3();} while( u8g.nextPage() ); //OLED를 통해 네번째 이미지를 출력
break; //스위치문 나가기
case 4: //selection이 4일 경우에
digitalWrite(4, 0); //세번째 LED의 불을 끄고
mySensor.update(); //온습도센서의 값을 불러오며
u8g.firstPage(); do {draw4();} while( u8g.nextPage() ); //OLED를 통해 다섯번째 이미지를 출력
delay(2000); //2초간 대기
selection=0; //selection을 다시 0으로
break;}} //스위치문 나가기
void draw0() {u8g.drawBitmapP(0, 0, 16, 64, temp);} //OLED 구동함수
void draw1() {u8g.drawStr(33, 32, “PC”);}
void draw2() {u8g.drawStr(33, 32, “ABS”);}
void draw3() {u8g.drawStr(33, 32, “PLA”);}
void draw4() {u8g.drawStr(20, 22, “T:”);
u8g.drawStr(20, 60, “H:”);
u8g.setPrintPos(50, 22);
u8g.print(mySensor.temperatureC);
u8g.setPrintPos(50, 60);
u8g.print(mySensor.humidity);}

#define F_CPU 16000000UL
#define sbi(PORTX,bitX) PORTX|=(1<<bitX)
#define cbi(PORTX,bitX) PORTX&=~(1<<bitX)

#include <mega128.h>
#include <delay.h>
#include <stdio.h>

void LCD_init(void);
void LCD_data(char data);
void LCD_command(char command);
void LCD_string(char line, char *string);
int Adc_Channel(char Adc_input);
void USART_Transmit(char data);
void USART_Transmit_int(int data);

char buf[16] = {‘ ‘,};
int adc_data = 0;
int cnt_ms = 0;
float cnt_s = 0.000;
float cnt_m = 0.000;
float cnt_data = 0.000;
float bpm = 0.000;
int bef_flag = 0;
int flag = 0;
int tick = 0;
int tick_flag = 0;

void main()
{
// io 핀 설정
DDRA = 0xFF;
DDRB = 0xFF;
DDRC = 0xFF;
PORTA = 0×00;
PORTB = 0×00;
PORTC = 0×00;

// ADC
ADMUX = 0×00;
ADCSRA = (1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);

// TIMER/COUNTER
TCCR0 = (1<<CS02)|(1<<CS00); // 128분주
TCNT0 = 256-125; // 125번 => 0.001s
TIMSK = (1<<TOIE0);

// UART
UCSR1A = 0×00;
UCSR1B = (1<<TXEN1);
UCSR1C = (1<<UCSZ11)|(1<<UCSZ10); //8bit
UBRR1H = 0;
UBRR1L = 103;

// LCD
LCD_init();
LCD_string(1,” BPM “);

#asm(“sei”)

// 동작 시작 부저 알림 “삡,삡”
sbi(PORTC,0);
delay_ms(100);
cbi(PORTC,0);
delay_ms(300);
sbi(PORTC,0);
delay_ms(100);
cbi(PORTC,0);

while(1)
{

adc_data = Adc_Channel(0);
bef_flag = flag;

if(adc_data > 500) // 기준치 보다 전압 신호 크면
{
sbi(PORTB,1);
flag = 1;
}

else
{
cbi(PORTB,1);
flag = 0;
}

if((bef_flag == 0) && (flag == 1)) // 기준치 보다 전압신호 높아지는 순간
{
cnt_data = (float)cnt_ms; // 카운트된 값 저장 . 심박 신호간 길이 (밀리초)
cnt_ms = 0;

cnt_s = cnt_data/1000; // 심박 신호간 길이 (초)
cnt_m = cnt_s/60; // 심박 신호간 길이 (분)
bpm = 1/cnt_m; // 분당 심박수 (주기와 주파수는 역수관계)
}

if(tick_flag == 1) // 2초에 한번 동작 하는 곳
{
//LCD 표시
sprintf(buf, ” %4d “,(int)bpm);
LCD_string(2,buf);

//블루투스 전송
USART_Transmit_int(bpm);
USART_Transmit(0);
//부저 동작
if ((bpm < 50) || (bpm > 180)) // 이상 BPM
{
sbi(PORTC,0);
USART_Transmit(‘ ‘);
USART_Transmit(‘W’);
USART_Transmit(‘A’);
USART_Transmit(‘R’);
USART_Transmit(‘N’);

USART_Transmit(‘ ‘);
USART_Transmit(‘S’);
USART_Transmit(‘T’);
USART_Transmit(‘A’);
USART_Transmit(‘T’);
USART_Transmit(‘E’);
}
else
cbi(PORTC,0);

tick_flag = 0;
}
}
}

interrupt [TIM0_OVF] void timer0_ovf_isr(void)
{
cnt_ms ++; // 0.001s 1씩 증가

tick ++;
if(tick >= 2000)
{
tick = 0;
tick_flag = 1;
}

TCNT0 = 256-125;
}

int Adc_Channel(char Adc_input)
{
ADMUX = 0b01000000 | Adc_input;
ADCSRA |= 0×40;
while((ADCSRA & 0×10) == 0);
return ADCW;
}

void LCD_command(char command)
{
PORTA = (command&0xF0); // send High data
cbi(PORTA,0); // RS=0
//cbi(PORTA,1); // RW=0
sbi(PORTA,2); // Enable
delay_us(1);
cbi(PORTA,2); // Disable

PORTA = (command&0x0F)<<4; // send Low data
cbi(PORTA,0); // RS=0
//cbi(PORTA,1); // RW=0
sbi(PORTA,2); // Enable
delay_us(1);
cbi(PORTA,2); // Disable
}

void LCD_data(char data)
{
delay_us(100);

PORTA = (data&0xF0); // send High data
sbi(PORTA,0); // RS=1
//cbi(PORTA,1); // RW=0
sbi(PORTA,2); // Enable
delay_us(1);
cbi(PORTA,2); // Disable

PORTA = (data&0x0F)<<4; // send Low data
sbi(PORTA,0); // RS=1
//cbi(PORTA,1); // RW=0
sbi(PORTA,2); // Enable
delay_us(1);
cbi(PORTA,2); // Disable
}

void LCD_init(void)
{
delay_ms(50);

LCD_command(0×28); // DL=0(4bit) N=1(2Line) F=0(5×7)
delay_ms(2); // [function set] 0b00101000
// 4:(DL) 1이면 8bit모드, 0이면 4bit모드
// 3:(N) 0이면 1줄짜리, 1이면 2줄짜리
// 2:(F) 0이면 5x8dots, 1이면 5x11dots

LCD_command(0x0C); // LCD ON, Cursor X, Blink X
delay_ms(2); // [display on/off control] 0b00001100
// 2:(D) 1이면 display on, 0이면 off
// 1:(C) 1이면 cursor on, 0이면 off
// 0:(B) 1이면 cursor blink, 0이면 off

LCD_command(0×06); // Entry Mode
delay_ms(2); // [entry mode set] 0b00000110
// 1:(I/D) 1이면 오른쪽으로, 0이면 왼쪽
// 0:(SH) CGRAM 사용관련

LCD_command(0×01); // LCD Clear
delay_ms(2);
}
void LCD_string(char line, char *string)
{
LCD_command(0×80+((line-1)*0×40));
while(*string)
LCD_data(*string++);
}

void USART_Transmit(char data)
{
while(!(UCSR1A & 0×20));
UDR1 = data;
}

void USART_Transmit_int(int data)
{
int temp;

if(data < 10)
{
temp = data;
USART_Transmit(temp+48);
}
else if(data < 100)
{
temp = data/10;
USART_Transmit(temp+48);
temp = (data%10)/1;
USART_Transmit(temp+48);
}
else if(data < 1000)
{
temp = (data%1000)/100;
USART_Transmit(temp+48);
temp = (data%100)/10;
USART_Transmit(temp+48);
temp = (data%10)/1;
USART_Transmit(temp+48);
}
else
{
temp = data/1000;
USART_Transmit(temp+48);
temp = (data%1000)/100;
USART_Transmit(temp+48);
temp = (data%100)/10;
USART_Transmit(temp+48);
temp = (data%10)/1;
USART_Transmit(temp+48);
}
}