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);

}