From 6f347f3ce91599e29f12ea23feec35c293e63124 Mon Sep 17 00:00:00 2001 From: Yuuichi Akagawa Date: Sun, 1 Jul 2012 18:38:04 +0900 Subject: [PATCH 1/1] initial commit --- AKBrobot/AKBrobot.cpp | 175 ++++++++++++++++++++++ AKBrobot/AKBrobot.h | 74 +++++++++ AKBrobot/examples/AKBrobot_ADB/AKBrobot_ADB.pde | 97 ++++++++++++ AKBrobot/examples/AKBrobot_ADK/AKBrobot_ADK.pde | 88 +++++++++++ AKBrobot/examples/AKBrobot_UART/AKBrobot_UART.pde | 95 ++++++++++++ README | 4 + 6 files changed, 533 insertions(+) create mode 100644 AKBrobot/AKBrobot.cpp create mode 100644 AKBrobot/AKBrobot.h create mode 100644 AKBrobot/examples/AKBrobot_ADB/AKBrobot_ADB.pde create mode 100644 AKBrobot/examples/AKBrobot_ADK/AKBrobot_ADK.pde create mode 100644 AKBrobot/examples/AKBrobot_UART/AKBrobot_UART.pde create mode 100644 README diff --git a/AKBrobot/AKBrobot.cpp b/AKBrobot/AKBrobot.cpp new file mode 100644 index 0000000..d69a97c --- /dev/null +++ b/AKBrobot/AKBrobot.cpp @@ -0,0 +1,175 @@ +/* + Copyright 2012 JAG-AKIBA + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include "AKBrobot.h" + +AKBrobot::AKBrobot() +{ +} + +void AKBrobot::init() +{ + //Initialize Motors + motors[0].in1 = MOTOR1A; + motors[0].in2 = MOTOR1B; + motors[0].pwm = MOTOR1P; + motors[1].in1 = MOTOR2A; + motors[1].in2 = MOTOR2B; + motors[1].pwm = MOTOR2P; + for(int i=0; i<_MAX_MOTORS; i++){ + pinMode(motors[i].in1, OUTPUT); + pinMode(motors[i].in2, OUTPUT); + digitalWrite(motors[i].in1, LOW); + digitalWrite(motors[i].in2, LOW); + } + + //Initialize Relays + relays[0] = RELAY1; + relays[1] = RELAY2; + pinMode(RELAY1, OUTPUT); + pinMode(RELAY2, OUTPUT); + + //Initialize Servos + servos[0].attach(SERVO1); + servos[0].write(90); + servolist[0] = SERVO1; +} + +//Primitive port assignment +// data[1] : 0x00-0x3f : digital port +// 0x40-0x7f : analog port +// 0x80-0xbf : Output digital port(PWM) / Input analog port + +//Configure primitive port +// data[3] : 0 : digital out +// 1 : analog out(PWM) +// 2 : digital in +// 3 : analog in +void AKBrobot::configPort(uint8_t *data) +{ + int portno; + +//Servo check + for(uint8_t i=0; i<_MAX_SERVOS; i++){ + if( servolist[i] == (data[1] & 0x3f) ){ + servos[i].detach(); + } + } + + switch(data[3]){ + case 0 : //digital out + if( data[1] < 0x40 ){ // digital port + portno = data[1]; + }else if( data[1] < 0x80 ){ // analog port + portno = (data[1] & 0x3f) + A0; + } + pinMode(portno, OUTPUT); + break; + case 1 : + //Not need + break; + case 2 : + if( data[1] < 0x40 ){ // digital port + portno = data[1]; + }else if( data[1] < 0x80 ){ // analog port + portno = (data[1] & 0x3f) + A0; + } + pinMode(portno, INPUT); + break; + case 3 : + //Not need + break; + defalut : + break; + } +} + +//Input from primitive port +int AKBrobot::inPort(uint8_t *data) +{ + int pindata = 0; + int portno; + uint8_t outdata[4]; + if( data[1] < 0x40 ){ // digital port + pindata = digitalRead(data[1]); + }else if( data[1] < 0x80 ){ // analog port(digital mode) + portno = (data[1] & 0x3f) + A0; + pindata = digitalRead(portno); + }else if( data[1] < 0xc0 ){ // analog port(analog mode) + portno = (data[1] & 0x3f) + A0; + pindata = analogRead(portno); + }else{ + } + + outdata[0] = data[0]; + outdata[1] = data[1]; + outdata[2] = (uint8_t)((pindata >> 8) & 0xff); + outdata[3] = (uint8_t)(pindata & 0xff); +// Serial.write(outdata, 4); + return(pindata); +} + +//Output to primitive port +void AKBrobot::outPort(uint8_t *data) +{ + if( data[1] < 0x40 ){ // digital port + digitalWrite(data[1], data[3] ? HIGH : LOW); + }else if( data[1] < 0x80 ){ // analog port + int portno = (data[1] & 0x3f) + A0; + digitalWrite(portno, data[3] ? HIGH : LOW); + }else if( data[1] < 0xc0 ){ // digital port(PWM) + int portno = (data[1] & 0x3f); + analogWrite(portno, data[3]); + }else{ + } +} + +//Motor control +void AKBrobot::ctrlMotor(uint8_t *data) +{ + if( data[1] < _MAX_MOTORS ){ + if( data[3] == 0) { //STOP + digitalWrite(motors[data[1]].in1, LOW); + digitalWrite(motors[data[1]].in2, LOW); + digitalWrite(motors[data[1]].pwm, 0); + }else{ + if( data[2] == 0){ //³“] + digitalWrite(motors[data[1]].in1, HIGH); + digitalWrite(motors[data[1]].in2, LOW); + }else{ //‹t“] + digitalWrite(motors[data[1]].in1, LOW); + digitalWrite(motors[data[1]].in2, HIGH); + } + analogWrite(motors[data[1]].pwm, data[3]); + } + } +} + +//Relay control +void AKBrobot::ctrlRelay(uint8_t *data) +{ + if( data[1] < _MAX_RELAYS ){ + digitalWrite(relays[data[1]], data[3] ? HIGH : LOW); + } +} + +//ServoMotor control +void AKBrobot::ctrlServo(uint8_t *data) +{ + if( data[1] < _MAX_SERVOS ){ + servos[data[1]].write(map(data[3], 0, 255, 0, 180)); + } +} + diff --git a/AKBrobot/AKBrobot.h b/AKBrobot/AKBrobot.h new file mode 100644 index 0000000..91fc810 --- /dev/null +++ b/AKBrobot/AKBrobot.h @@ -0,0 +1,74 @@ +/* + Copyright 2012 JAG-AKIBA + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#if !defined(_AKBROBOT_H_) +#define _AKBROBOT_H_ + +#include "Servo.h" + +#if defined(ARDUINO) && ARDUINO >=100 +#include +#else +#include +#endif + + +//Core Module pin assign +#define _MAX_MOTORS 2 +#define _MAX_RELAYS 2 +#define _MAX_SERVOS 1 + +#define RELAY1 2 +#define RELAY2 4 +#define MOTOR1A A0 +#define MOTOR1B A1 +#define MOTOR1P 3 +#define MOTOR2A A2 +#define MOTOR2B A3 +#define MOTOR2P 5 +#define SERVO1 6 + +//Static classes +//Motors +class Motors { +public: + int in1; + int in2; + int pwm; + Motors(){}; + Motors(int in1, int in2, int pwm) : in1(in1), in2(in2), pwm(pwm){}; +}; + + +class AKBrobot +{ +private: + Motors motors[_MAX_MOTORS]; + int relays[_MAX_RELAYS]; + Servo servos[_MAX_SERVOS]; + uint8_t servolist[_MAX_SERVOS]; + +public: + AKBrobot(); + void init(); + void configPort(uint8_t *data); + int inPort(uint8_t *data); + void outPort(uint8_t *data); + void ctrlMotor(uint8_t *data); + void ctrlRelay(uint8_t *data); + void ctrlServo(uint8_t *data); +}; + +#endif //_AKBROBO_H_ diff --git a/AKBrobot/examples/AKBrobot_ADB/AKBrobot_ADB.pde b/AKBrobot/examples/AKBrobot_ADB/AKBrobot_ADB.pde new file mode 100644 index 0000000..35f4d3c --- /dev/null +++ b/AKBrobot/examples/AKBrobot_ADB/AKBrobot_ADB.pde @@ -0,0 +1,97 @@ +/* + Copyright 2012 JAG-AKIBA + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include +#include +#include "AKBrobot.h" + +void setup(); +void loop(); + +// Adb connection. +Connection * connection; + +boolean flgReply = false; +uint8_t dataReply[4]; + +AKBrobot robo; +//Initialize +void setup() +{ + robo.init(); + + // Initialize the ADB subsystem. + ADB::init(); + + // Open an ADB stream to the phone's shell. Auto-reconnect + connection = ADB::addConnection("tcp:4567", true, adbEventHandler); +} + +//Main loop +void loop() +{ + // Poll the ADB subsystem. + ADB::poll(); + if(flgReply == true ){ + connection->write(4, dataReply); + flgReply = false; + } +} + + +// Event handler for the shell connection. +void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data) +{ + // Data packets contain four bytes + // +0 : Command + // +1 : Sub command + // +2 : Data1 + // +3 : Data2 + if (event == ADB_CONNECTION_RECEIVE) + { + int val; + switch(data[0]){ + case 0: //プリミティブ設定モード + robo.configPort(data); + break; + case 1: //プリミティブ出力 + robo.outPort(data); + break; + case 2: //プリミティブ入力 + val = robo.inPort(data); +// data[2] = (byte)(val >> 8); +// data[3] = (byte)(val & 0xff); +// connection->write(4, data); + dataReply[0] = data[0]; + dataReply[1] = data[1]; + dataReply[2] = (uint8_t)(val >> 8); + dataReply[3] = (uint8_t)(val & 0xff); + flgReply = true; + + break; + case 3: //モータ制御 (data[1]:モータ番号, data[2],data[3]:データ) + robo.ctrlMotor(data); + break; + case 4: //リレー制御 (data[1]:リレー番号, data[2],data[3]:データ) + robo.ctrlRelay(data); + break; + case 5: //サーボ制御 (data[1]:サーボ番号, data[2],data[3]:データ) + robo.ctrlServo(data); + break; + default: + break; + } + } +} diff --git a/AKBrobot/examples/AKBrobot_ADK/AKBrobot_ADK.pde b/AKBrobot/examples/AKBrobot_ADK/AKBrobot_ADK.pde new file mode 100644 index 0000000..efb2196 --- /dev/null +++ b/AKBrobot/examples/AKBrobot_ADK/AKBrobot_ADK.pde @@ -0,0 +1,88 @@ +/* + Copyright 2012 JAG-AKIBA + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include +#include +#include +#include +#include "AKBrobot.h" + +void setup(); +void loop(); +AndroidAccessory acc("JAG-AKIBA", + "CoreModule01", + "JAG-AKIBA MotorDriver", + "1.0", + "http://android-akihabara.info", + "0000000000000001"); +boolean isConn = false; + +AKBrobot robo; +//Initialize +void setup() +{ + robo.init(); + //Start USB + acc.powerOn(); +} + +//Main loop +void loop() +{ + byte data[4]; + static byte count = 0; + + if (acc.isConnected()) { + if( isConn == false ) isConn = true; + + int len = acc.read(data, sizeof(data), 1); + int val; + //Write function + if (len > 0) { + // assumes only one command per packet + switch(data[0]){ + case 0: //プリミティブ設定モード + robo.configPort(data); + break; + case 1: //プリミティブ出力 + robo.outPort(data); + break; + case 2: //プリミティブ入力 + val = robo.inPort(data); + data[2] = (byte)(val >> 8); + data[3] = (byte)(val & 0xff); + acc.write(data, 4); + break; + case 3: //モータ制御 (data[1]:モータ番号, data[2],data[3]:データ) + robo.ctrlMotor(data); + break; + case 4: //リレー制御 (data[1]:リレー番号, data[2],data[3]:データ) + robo.ctrlRelay(data); + break; + case 5: //サーボ制御 (data[1]:サーボ番号, data[2],data[3]:データ) + robo.ctrlServo(data); + break; + default: + break; + } + } + }else{ + if( isConn == true ){ + isConn == false; + robo.init(); + } + } + delay(5); +} diff --git a/AKBrobot/examples/AKBrobot_UART/AKBrobot_UART.pde b/AKBrobot/examples/AKBrobot_UART/AKBrobot_UART.pde new file mode 100644 index 0000000..d3ef116 --- /dev/null +++ b/AKBrobot/examples/AKBrobot_UART/AKBrobot_UART.pde @@ -0,0 +1,95 @@ +/* + Copyright 2012 JAG-AKIBA + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. +*/ +#include +#include "AKBrobot.h" +#define RECVTIMEOUT 100 //ms + +void setup(); +void loop(); +uint8_t getdata(byte *buf, size_t len); + + +AKBrobot robo; +//Initialize +void setup() +{ + robo.init(); + Serial.begin(115200); +} + +//Main loop +void loop() +{ + byte data[4]; + static byte count = 0; + + int len = getdata(data, sizeof(data)); + int val; + //Write function + if (len > 0) { + // assumes only one command per packet + switch(data[0]){ + case 0: //プリミティブ設定モード + robo.configPort(data); + break; + case 1: //プリミティブ出力 + robo.outPort(data); + break; + case 2: //プリミティブ入力 + val = robo.inPort(data); + data[2] = (byte)(val >> 8); + data[3] = (byte)(val & 0xff); + Serial.write(data, 4); + break; + case 3: //モータ制御 (data[1]:モータ番号, data[2],data[3]:データ) + robo.ctrlMotor(data); + break; + case 4: //リレー制御 (data[1]:リレー番号, data[2],data[3]:データ) + robo.ctrlRelay(data); + break; + case 5: //サーボ制御 (data[1]:サーボ番号, data[2],data[3]:データ) + robo.ctrlServo(data); + break; + default: + break; + } + } + delay(5); +} + +uint8_t getdata(byte *buf, size_t len) +{ + char c; + size_t i; + unsigned long starttime; + + if (Serial.available() < 0) { + return 0; + } + starttime = millis(); + + for(i=0; i 0) { + c = Serial.read(); + buf[i] = (byte)c; + }else{ + if( millis() - starttime > RECVTIMEOUT ) { //Timeout + return 0; + } + } + } + return (i - 1); +} diff --git a/README b/README new file mode 100644 index 0000000..82f9d54 --- /dev/null +++ b/README @@ -0,0 +1,4 @@ +日本Androidの会 秋葉原支部 ロボット部 +コアモジュール(モータドライバー) +・Android - 操作用Androidアプリ +　必要なアプリソースをimport->ExistingProjects into Workspaceで取り込む -- 2.11.0