Tải bản đầy đủ - 0 (trang)
TÀI LIỆU THAM KHẢO

TÀI LIỆU THAM KHẢO

Tải bản đầy đủ - 0trang

PHỤ LỤC 1

MÃ NGUỒN BỘ ĐIỀU KHIỂN

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

* % PROJECT MECHNLU WALLBOT - NONG LAM UNIVERSITY HO CHI MINH CITY %

* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

*

This product create by Team Mechatronics DH14CD

*

- TRAN DUONG HOAN

14153016

*

- CHAU KHANH DAT

14153011

*

Begin in August 5, 2017

*

All code publish in https://portal.edu.vn key Wall-Bot

* -----------------------------------------------------------------#include

#include "SH1106Wire.h"

#include ;

#include

#include "image.h"



*

*

*

*

*

*

*

*

*

*/



String menuHome[10] = {"- MechNLU WallBot Menu -", "Wifi Config", "Set

EDF/PWM", "System parameters", "About us", "Exit"};

String menuWifi[10] = {"- Wifi Config -", "Set SSID", "Set Password",

"Exit"};

String menuSystem[10] = {"- System Config -", "Set VRef", "Resistors",

"Wheel diameter", "PPR define", "Differrence", "Battery warning", "Exit"};

String menuRes[10] = {"- Resistor Config -", "Limit resistor", "Load

resistor", "Exit"};

String menuBat[10] = {"- Battery Config -", "Highest voltage", "Lowest

Voltage", "Voltage warning", "Exit"};

String menuSpeed[10] = {"- Speed Config -", "Set EDF value", "Set max of

PWM", "Exit"};

String menuList[10] = menuHome;

byte numItem = 5;

byte item = 1; bool Is_Connecting = false;

SH1106Wire display(0x3c, D3, D4);

#define

#define

#define

#define

#define

#define

#define

#define

#define

#define



byte menu = 0;



BT1 D2

BT2 D5

BT3 D1

YSC D0

XSC D6

JSK A0

DOWN 0

UP 1

LEFT 2

RIGHT 3



short int x_value = 0; short int y_value = 0;

short int x_tmp = 0;

short int y_tmp = 0;

IPAddress server_ip(192, 168, 4, 1);

WiFiClient client;

#define PORT 1102

float vr; float r1; float r2; float vmin; float vmax; float valr;

int EDF_SPEED; byte max_PWM; byte error_PWM; int PPR; float Dr;

void setup() {

Serial.begin(115200);

EEPROM.begin(512);

// put your setup code here, to run once:

pinMode(BT1, INPUT_PULLUP);pinMode(BT2, INPUT_PULLUP);

pinMode(BT3, INPUT_PULLUP);pinMode(JSK, INPUT);



pinMode(YSC, OUTPUT); pinMode(XSC, OUTPUT);

digitalWrite(YSC, 0); digitalWrite(XSC, 1);

display.init();

display.flipScreenVertically();

display.setFont(ArialMT_Plain_10);

WifiConnect();

display.clear();

display.setTextAlignment(TEXT_ALIGN_LEFT);

display.setFont(ArialMT_Plain_10);

vr = EEPROM.read(101) +

r1 = EEPROM.read(102) +

r2 = EEPROM.read(103) +

vmax = EEPROM.read(105)

vmax /= 100;

vmin = EEPROM.read(106)

vmin /= 100;

valr = EEPROM.read(107)

valr /= 100;

vr /= 100;

r1 /= 10;

r2 /= 10;



}



300;

450;

700;

+ 1200;

+ 900;

+ 900;



EDF_SPEED = EEPROM.read(100) * 10;

max_PWM = EEPROM.read(120);

error_PWM = EEPROM.read(108);

Dr = EEPROM.read(104) + 3250;

Dr /= 100;

PPR = EEPROM.read(109);

PPR = 2000 + PPR * 5;



#include

#include

#include

#include



"res.h"

"syst.h"

"vol.h"

"wifi.h"



void WifiConnect() {

static unsigned long timer_connect = millis();

static byte numc = 1;

if (Is_Connecting) {

if (millis() - timer_connect > 500) {

if (WiFi.status() != WL_CONNECTED) {

display.clear();

display.setTextAlignment(TEXT_ALIGN_LEFT);

display.drawString(15, 0, "MechNLU WallBot 3.5");

clearRow(50);

if (numc == 1) {

display.drawString(23, 50, "Wifi - Connecting.");

} else if (numc == 2) {

display.drawString(23, 50, "Wifi - Connecting..");

} else if (numc == 3) {

display.drawString(23, 50, "Wifi - Connecting...");

} else if (numc == 4) numc = 0;

numc++;

display.drawXbm(34, 14, WiFi_Logo_width, WiFi_Logo_height,

WiFi_Logo_bits);

display.display();

} else {

clearRow(0);

display.drawString(13, 0, "WallBot is ready to go");



clearRow(50);

String msg = "IP address: ";

msg += WiFi.localIP().toString();

display.drawString(10, 50, msg);

display.display();

delay(2000);

Is_Connecting = false;

}

timer_connect = millis();



}



}

} else {

WiFi.mode(WIFI_STA);

WiFi.disconnect(true);

String chSSID = "";

for (byte i = 0; i < 16; i++) {

byte tmp = EEPROM.read(i);

if (tmp >= 32 && tmp <= 126) chSSID += char(tmp);

}

char ssid[chSSID.length() + 1];

chSSID.toCharArray(ssid, chSSID.length() + 1);

String chPASS = "";

for (byte _i = 16; _i < 32; _i++) {

byte tmp = EEPROM.read(_i);

if (tmp >= 32 && tmp <= 126) chPASS += char(tmp);

}

char password[chPASS.length() + 1];

chPASS.toCharArray(password, chPASS.length() + 1);

Serial.println(ssid);

Serial.println(password);

WiFi.begin(ssid, password);

Is_Connecting = true;

}



void clearRow(byte i) {

display.setColor(BLACK);

for (byte _i = i; _i < i + 14; _i++) {

display.drawLine(0, _i, 128, _i);

}

display.setColor(WHITE);

}

void fillRow(byte i) {

display.setColor(WHITE);

for (byte _i = i; _i < i + 14; _i++) {

display.drawLine(0, _i, 128, _i);

}

}

void showAbout() {

display.setTextAlignment(TEXT_ALIGN_CENTER);

short int i = 64;

while (!keyDown(BT3)) {

display.setColor(BLACK);

for (byte _i = 19; _i < 64; _i++) {

display.drawLine(0, _i, 128, _i);

}

display.setColor(WHITE);

display.drawString(128 / 2, i, "This is a project");

display.drawString(128 / 2, i + 14, "of Mechatronics");

display.drawString(128 / 2, i + 14 * 2, "-><-");

display.drawString(128 / 2, i + 14 * 3, "Faculty of");



display.drawString(128 / 2, i + 14 * 4, "Engineering & Technology");

display.drawString(128 / 2, i + 14 * 5, "Nong Lam University");

display.drawString(128 / 2, i + 14 * 6, "Ho Chi Minh City");

display.drawString(128 / 2, i + 14 * 7, "-><-");

display.drawString(128 / 2, i + 14 * 8, "Author");

display.drawString(128 / 2, i + 14 * 9 - 5, "__________");

display.drawString(128 / 2, i + 14 * 10, "14153016");

display.drawString(128 / 2, i + 14 * 11, "TRAN DUONG HOAN");

display.drawString(128 / 2, i + 14 * 13, "14153011");

display.drawString(128 / 2, i + 14 * 14, "CHAU KHANH DAT");

display.drawString(128 / 2, i + 14 * 15 - 5, "__________");

display.drawString(128 / 2, i + 14 * 16, "-><-");

display.drawString(128 / 2, i + 14 * 17, "MechNLU WallBot 3.5");

display.drawString(128 / 2, i + 14 * 18, "-><-");

display.drawString(128 / 2, i + 14 * 19, "Thank you");

display.drawString(128 / 2, i + 14 * 20, "<3");

i--;

display.setColor(BLACK);

for (byte _i = 0; _i < 19; _i++) {

display.drawLine(0, _i, 128, _i);

}

display.setColor(WHITE);

display.drawString(128 / 2, 0, "About us");

for (byte _i = 15; _i < 17; _i++) {

display.drawLine(0, _i, 128, _i);

}

display.display();

if (i == -(20 * 14)) i = 64;

delay(10);

}

}

void showMenu() {

display.clear();

ref_menu();

while (1) {

ref_menu();

switch (menu) {

case 0: // Menu Home

if (keyDown(BT3)) switch (item) {

case 1:

memcpy(menuList, menuWifi, 10 * sizeof(String));

numItem = 3;

menu = 1;

item = 1;

break;

case 2:

memcpy(menuList, menuSpeed, 10 * sizeof(String));

numItem = 3;

menu = 5;

item = 1;

break;

case 3:

memcpy(menuList, menuSystem, 10 * sizeof(String));

numItem = 7;

menu = 2;

item = 1;

break;

case 4:

showAbout();

break;

case 5:



item = 1;

return;

break;

}

break;

case 1: // Menu Wifi

if (keyDown(BT3)) switch (item) {

case 1:

saveSSID();

break;

case 2:

savePASS();

break;

case 3:

memcpy(menuList, menuHome, 10 * sizeof(String));

numItem = 5;

menu = 0;

item = 1;

break;

}

break;

case 2: // Menu System

if (keyDown(BT3)) switch (item) {

case 1:

setVref();

break;

case 2:

memcpy(menuList, menuRes, 10 * sizeof(String));

numItem = 3;

menu = 3;

item = 1;

break;

case 3:

setD();

break;

case 4:

setPPR();

break;

case 5:

setDif();

break;

case 6:

memcpy(menuList, menuBat, 10 * sizeof(String));

numItem = 4;

menu = 4;

item = 1;

break;

case 7:

memcpy(menuList, menuHome, 10 * sizeof(String));

numItem = 5;

menu = 0;

item = 2;

break;

}

break;

case 3: // Menu Resistors

if (keyDown(BT3)) switch (item) {

case 1:

setRes1();

break;

case 2:



setRes2();

break;

case 3:

memcpy(menuList, menuSystem, 10 * sizeof(String));

numItem = 7;

menu = 2;

item = 2;

break;



}

break;

case 4: // Menu Battery

if (keyDown(BT3)) switch (item) {

case 1:

setVol1();

break;

case 2:

setVol2();

break;

case 3:

setVol3();

break;

case 4:

memcpy(menuList, menuSystem, 10 * sizeof(String));

numItem = 7;

menu = 2;

item = 6;

break;

}

break;

case 5:

// Menu Speed;

if (keyDown(BT3)) switch (item) {

case 1:

saveSPEED();

break;

case 2:

saveSPEED2();

break;

case 3:

memcpy(menuList, menuHome, 10 * sizeof(String));

numItem = 5;

menu = 0;

item = 2;

break;

}

break;

}

byte btn = checkChoose();

if (btn == DOWN) {

if (item < numItem) item++;

}

if (btn == UP) {

if (item > 1) item--;

}

xy_read();

display.display();



}

}

byte checkChoose() {

static bool stt[] = {0, 0, 0, 0};

if (y_value < 5) stt[1] = 0;



if (y_value > -5) stt[0] = 0;

if (x_value < 5) stt[3] = 0;

if (x_value > -5) stt[2] = 0;

bool change = 0;

if (y_value >= 5 && !stt[1]) {

stt[1] = 1;

change = 1;

}

if (y_value <= -5 && !stt[0]) {

stt[0] = 1;

change = 1;

}

if (x_value >= 5 && !stt[3]) {

stt[3] = 1;

change = 1;

}

if (x_value <= -5 && !stt[2]) {

stt[2] = 1;

change = 1;

}

if (change == 0) return 5;

for (byte i = 0; i < 4; i++) {

if (stt[i] == 1) return i;

}

}

void ref_menu() {

clearRow(0);

clearRow(4 * 14 + 5);

display.setTextAlignment(TEXT_ALIGN_CENTER);

display.setColor(WHITE);

for (byte _i = 15; _i < 17; _i++) {

display.drawLine(0, _i, 128, _i);

}

display.drawString(128 / 2, 0, menuList[0]);

display.setTextAlignment(TEXT_ALIGN_LEFT);

for (byte i = 1; i < 4; i++) {

clearRow(i * 14 + 5);

display.drawString(5, i * 14 + 5, menuList[(int)((item - 1) / 3) * 3 +

i]);

}

int rowchoose = item;

while (rowchoose > 3) {

rowchoose -= 3;

}

fillRow(rowchoose * 14 + 5);

display.setColor(BLACK);

display.drawString(5, rowchoose * 14 + 5, menuList[item]);

display.setColor(WHITE);

display.display();

}

char * TimeToString(unsigned long t) {

static char str[12];

long h = t / 3600;

t = t % 3600;

int m = t / 60;

int s = t % 60;

if (h > 999) sprintf(str, "%04d:%02d:%02d", h, m, s);

else if (h > 99) sprintf(str, "%03d:%02d:%02d", h, m, s);

else sprintf(str, "%02d:%02d:%02d", h, m, s);



return str;

}

void loop() {

//btn_check();

//if (Is_Menu) return;

if (WiFi.status() == WL_CONNECTED && !Is_Connecting) {

static unsigned long timerRSSI = millis();

if (millis() - timerRSSI > 1000) {

//showRSSI(getBarsSignal(WiFi.RSSI()));

timerRSSI = millis();

}

while (!client.connected()) {

if (!client.connect(server_ip, PORT)) {

Serial.println("connection failed");

delay(1000);

return;

} else {

client.println("R=0|0");

display.clear();

}

}

if (client.available()) {

static String str = "";

char c = client.read();

str += c;

if (c == '\r') {

str.replace("\r", "");

str.replace("\n", "");

float bat = str.substring(0, str.indexOf("|")).toFloat();

bat = vr * (bat / 1023);

bat = bat / r1 * (r1 + r2);

String msg = "Battery: ";

msg += constrain(int((bat - vmin) / (vmax - vmin) * 100), 0, 100);

msg += "% ";

if (bat < valr) msg += "(Running out)";

clearRow(0);

display.drawString(0, 0, msg);

static float old_d1 = 0;

static float old_d2 = 0;

static float d = 0;

str = str.substring(str.indexOf("|") + 1);

float d1 = str.substring(0, str.indexOf("|")).toFloat();

str = str.substring(str.indexOf("|") + 1);

float d2 = str.substring(0, str.indexOf("|")).toFloat();

float dp = float((abs(d1 - old_d1) + abs(d2 - old_d2)) / 2) / PPR *

Dr * 3.14 / 10;

d += dp;

old_d1 = d1;

old_d2 = d2;

msg = "Moved: ";

msg += d;

msg += " cm";

clearRow(15);

display.drawString(0, 15, msg);

msg = "Speed: ";

msg += dp;

msg += " cm/s";

clearRow(30);

display.drawString(0, 30, msg);



msg = "Timer: ";

msg += TimeToString(millis()/1000);

clearRow(45);

display.drawString(0, 45, msg);

display.display();

str = "";

}

}

if (keyDown(BT1)) {

client.print("E=");

client.println(EDF_SPEED);

}

if (keyDown(BT2)) {

client.println("E=1000");

}

if (keyDown(BT2)) {

client.println("R=0|0");

}

xy_read();

if (x_tmp != x_value || y_tmp != y_value) {

float lpwm = constrain(y_value + x_value, -10, 10);

float rpwm = constrain(y_value - x_value, -10, 10);

lpwm = map(lpwm, -10, 10, -max_PWM, max_PWM);

rpwm = map(rpwm, -10, 10, -max_PWM, max_PWM);

rpwm = rpwm < 0 ? rpwm + error_PWM : rpwm - error_PWM;

String str = "R=";

str += lpwm;

str += "|";

str += rpwm;

client.println(str);

x_tmp = x_value;

y_tmp = y_value;

}

} else {

if (keyDown(BT3)) {

showMenu();

}

WifiConnect();

}



// put your main code here, to run repeatedly:

display.display();

}

void xy_read() {

static bool read_x = true;

static unsigned long timer = millis();

if (millis() - timer > 10 && read_x) {

int tmp = 0;

for (byte i = 0; i < 5; i++) {

tmp += analogRead(A0);

}

x_value = map(tmp / 5, 0, 1024, -10, 10);

digitalWrite(YSC, 1);

digitalWrite(XSC, 0);

read_x = false;

timer = millis();

}



if (millis() - timer > 10 && !read_x) {

int tmp = 0;

for (byte i = 0; i < 5; i++) {

tmp += analogRead(A0);

}

y_value = map(tmp / 5, -20, 1024, 10, -10);

digitalWrite(XSC, 1);

digitalWrite(YSC, 0);

read_x = true;

timer = millis();

}

}

bool isPress(byte bt) {

return digitalRead(bt);

}

bool keyDown(byte bt) {

static bool keyDown[3] = {0, 0, 0};

static unsigned long timer = millis();

if (millis() - timer < 1) return false;

timer = millis();

if (!digitalRead(BT1)) keyDown[0] = 0;

if (!digitalRead(BT2)) keyDown[1] = 0;

if (digitalRead(BT3)) keyDown[2] = 0;

if (bt == BT1 && digitalRead(BT1) && !keyDown[0]) {

keyDown[0] = 1;

return 1;

}

if (bt == BT2 && digitalRead(BT2) && !keyDown[1]) {

keyDown[1] = 1;

return 1;

}

if (bt == BT3 && !digitalRead(BT3) && !keyDown[2]) {

keyDown[2] = 1;

return 1;

}

return false;

}

bool keyUP(byte bt) {

static bool keyUp[3] = {0, 0, 0};

static unsigned long timer = millis();

if (millis() - timer < 1) return false;

timer = millis();

if (digitalRead(BT1)) keyUp[0] = 0;

if (digitalRead(BT2)) keyUp[1] = 0;

if (!digitalRead(BT3)) keyUp[2] = 0;

if (bt == BT1 && !digitalRead(BT1) && !keyUp[0]) {

keyUp[0] = 1;

return 1;

}

if (bt == BT2 && !digitalRead(BT2) && !keyUp[1]) {

keyUp[1] = 1;

return 1;

}

if (bt == BT3 && digitalRead(BT3) && !keyUp[2]) {

keyUp[2] = 1;

return 1;

}

return false;

}



PHỤ LỤC 2

MÃ NGUỒN ROBOT LEO TƯỜNG

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

* % PROJECT MECHNLU WALLBOT - NONG LAM UNIVERSITY HO CHI MINH CITY %

* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

*

This product create by Team Mechatronics DH14CD

*

- TRAN DUONG HOAN

14153016

*

- CHAU KHANH DAT

14153011

*

Begin in August 5, 2017

*

Using Encoder Library https://github.com/PaulStoffregen/Encoder

* -----------------------------------------------------------------#include

#include

Servo EDF; Encoder lEnc(7, 2);

#define

#define

#define

#define

#define

#define

#define



Encoder rEnc(3, 4);



BAT A0

L1 10

L2 16

R1 14

R2 15

LM 5

RM 6



void setup() {

Serial1.begin(115200);

EDF.attach(9); delay(3000);

EDF.writeMicroseconds(1000);

pinMode(BAT, INPUT); pinMode(L1, OUTPUT);

pinMode(L2, OUTPUT); pinMode(R1, OUTPUT);

pinMode(R2, OUTPUT); pinMode(RM, OUTPUT);

pinMode(LM, OUTPUT);

}

void loop() {

String recv;

static unsigned long int mill = millis();

if (millis() - mill >= 1000) {

Serial1.print(analogRead(BAT));

Serial1.print("|");

Serial1.print(lEnc.read());

Serial1.print("|");

Serial1.println(-rEnc.read());

mill = millis();

}

if (Serial1.available()) recv = Serial1.readStringUntil('\r');

recv.replace("\r", "");

recv.replace("\n", "");

if (recv.substring(0, 2) == "E=") {

EDF.writeMicroseconds(recv.substring(2).toInt());

}

if (recv.substring(0, 2) == "R=") {

recv = recv.substring(2);

digitalWrite(L1, recv.substring(0, 1) == "-");

digitalWrite(L2, recv.substring(0, 1) != "-");

analogWrite(LM, byte(abs(recv.substring(0,

recv.indexOf("|")).toFloat())));

recv = recv.substring(recv.indexOf("|") + 1);

digitalWrite(R1, recv.substring(0, 1) != "-");

digitalWrite(R2, recv.substring(0, 1) == "-");

analogWrite(RM, byte(abs(recv.toFloat())));

}

}



*

*

*

*

*

*

*

*

*

*/



Tài liệu bạn tìm kiếm đã sẵn sàng tải về

TÀI LIỆU THAM KHẢO

Tải bản đầy đủ ngay(0 tr)

×