Sketch ロボットアーム
/*
ロボットアーム＋ボタンコントロール
PCA9685 16チャンネル PWM Servoモータードライバー（I2C）
ボタン用出力ピン：2〜9
*/
#include <PCA9685.h> //PCA9685用ヘッダーファイル
#include "U8glib.h"
PCA9685 pwm = PCA9685(0x40); //PCA9685のアドレス指定（アドレスジャンパ未接続時）
#define SERVOMIN 150 //最小パルス幅 (GWS Micro2BBMG)
#define SERVOMAX 500 //最大パルス幅 (GWS Micro2BBMG)
#define MAXCOM 4 // 取得コマンド数
#define BSIZE 64 // バッファサイズ
char com[MAXCOM][BSIZE];
U8GLIB_SSD1306_128X64 u8g(U8G_I2C_OPT_DEV_0 | U8G_I2C_OPT_NO_ACK | U8G_I2C_OPT_FAST); // Fast I2C / TWI
struct servoopt {
int8_t pmin;
int8_t pmax;
int8_t pnut;
int8_t pstp;
int8_t neg;
bool aut;
} sset[4] = {
-76, 42, 0, 2, 1, false,
-64, 40, -40, 1, 1, false,
-44, 84, -40, 1, 1, false,
-90, 90, 0, 2, -1, false
};
int8_t cp[4];
int8_t tp[4];
int8_t ap[][4] = {
0, -34, -15, 56,
40, -20, 41, 56,
-76, -20, 41, 56,
-76, -14, -24, 56,
-76, 24, -24, -58,
0, 24, -24, -58,
0, -40, -40, 0
};
char fmb[40];
void setup() {
Serial.begin(115200);
for (int i = 2; i <= 9; i++) pinMode(i, INPUT_PULLUP) ;
pinMode(10, INPUT_PULLUP) ;
pwm.begin(); //初期設定 (アドレス0x40用)
pwm.setPWMFreq(50); //PWM周期を50Hzに設定 (アドレス0x40用)
for(int i=0;i<4;i++) cp[i] = sset[i].pnut;
}
void loop() {
int val;
for (int i = 0; i < 4; i++) {
if (!sset[i].aut) {
val = getbutton(i) ;
if (val == 1) {
cp[i] += sset[i].pstp;
} else if (val == 2) {
cp[i] -= sset[i].pstp;
} else if (val == 3) {
sset[i].aut = true;
}
} else {
if (cp[i] > sset[i].pnut) cp[i] -= abs(sset[i].pstp);
else cp[i] += abs(sset[i].pstp);
if (abs(cp[i] - sset[i].pnut) < abs(sset[i].pstp)) sset[i].aut = false;
}
cp[i] = constrain(cp[i], sset[i].pmin, sset[i].pmax);
servo_write(i, cp[i] * sset[i].neg + 90);
/*
Serial.print(" ");
Serial.print(cp[i]);
*/
}
u8g_prepare();
u8g.firstPage();
do {
u8g_display();
} while ( u8g.nextPage() );
/*
GetStrings
*/
if (Serial.available() > 0) {
int stl = getSerialString(com[0]) ;
Serial.println(stl);
for (int i = 0; i < MAXCOM; i++) {
Serial.print(i);
Serial.print(" : ");
int val = atoi(com[i]);
Serial.println(val);
}
Serial.println();
if (stl == 4) {
for (int i = 0; i < 4; i++) {
tp[i] = constrain(atoi(com[i]), sset[i].pmin, sset[i].pmax);
}
moveToTergetPoint();
}
}
/*
* Auto Moving
*/
if(digitalRead(10) == 0) autoMove();
delay(1);
}
void autoMove() {
int num = sizeof(ap)/sizeof(ap[0]);
for (int j = 0; j < num; j++) {
for(int i = 0; i < 4; i++) tp[i] = ap[j][i];
moveToTergetPoint();
delay(1000);
}
}
void moveToTergetPoint() {
bool done;
u8g_prepare();
do {
done = true;
for (int i = 0; i < 4; i++) {
if (abs(cp[i] - tp[i]) >= abs(sset[i].pstp)) {
done = false;
if (cp[i] > tp[i]) cp[i] -= abs(sset[i].pstp);
else cp[i] += abs(sset[i].pstp);
cp[i] = constrain(cp[i], sset[i].pmin, sset[i].pmax);
servo_write(i, cp[i] * sset[i].neg + 90);
}
}
u8g.firstPage();
do u8g_display(); while ( u8g.nextPage());
delay(1);
} while (!done);
}
int getbutton(int no) {
int res;
/*
res = (digitalRead(no + 2) == 0) * 2;
res = res + (digitalRead(no + 6) == 0);
*/
res = (digitalRead(no * 2 + 2) == 0) << 1 ;
res = res + (digitalRead(no * 2 + 3) == 0);
return (res);
}
void servo_write(int ch, int ang) { //動かすサーボチャンネルと角度を指定
ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（150～500）に変換
pwm.setPWM(ch, 0, ang);
}
/*
OLED
*/
void u8g_prepare(void) {
u8g.setFont(u8g_font_6x10);
u8g.setFontRefHeightExtendedText();
u8g.setDefaultForegroundColor();
u8g.setFontPosTop();
}
#define BMAX 127
#define BMIN 38
#define BCEN (BMAX+BMIN)/2
#define BLEN BMAX-BMIN
void u8g_display() {
u8g.drawStr( 50, 0, "Controller");
for (int i = 0; i < 4; i++) {
sprintf(fmb, "%4d", cp[i]);
u8g.drawStr( 0, i * 12 + 18, fmb);
// Serial.print(fmb);
int len = abs(cp[i]) / 2 + 1;
if (cp[i] < 0) u8g.drawBox(BCEN - len, i * 12 + 18, len, 6);
else u8g.drawBox(BCEN, i * 12 + 18, len, 6);
}
// Serial.println("");
}
int getSerialString(char* ad) {
int ch;
int num = 0;
int pos = 0;
bool idle = true;
for (int i = 0; i < MAXCOM; i++) *(ad + i * BSIZE) = 0;
delay(10);
while (Serial.available() > 0) {
ch = Serial.read();
if (idle) {
if (ch > ' ') idle = false;
}
if (! idle && num < MAXCOM) {
if (ch > ' ') {
*(ad + pos) = ch;
pos++;
} else {
*(ad + pos) = 0;
ad += BSIZE;
pos = 0;
num++;
idle = true;
}
}
if (ch == 0x0a) break;
}
return (num);
}