update
This commit is contained in:
parent
8cc1013bee
commit
ac344bb427
4 changed files with 167 additions and 132 deletions
47
arduino/catchrobo2012/Motor.cpp
Normal file
47
arduino/catchrobo2012/Motor.cpp
Normal file
|
|
@ -0,0 +1,47 @@
|
||||||
|
/*
|
||||||
|
Motor.cpp
|
||||||
|
(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||||
|
*/
|
||||||
|
|
||||||
|
#if defined(ARDUINO) && ARDUINO >= 100
|
||||||
|
#include <Arduino.h>
|
||||||
|
#else
|
||||||
|
#include <WProgram.h>
|
||||||
|
#endif
|
||||||
|
#include "Motor.h"
|
||||||
|
|
||||||
|
void Motor::mode(byte mode){
|
||||||
|
digitalWrite(Motor::_pin1, (mode>>0)&1);
|
||||||
|
digitalWrite(Motor::_pin2, (mode>>1)&1);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Motor::mode(byte mode, byte speed){
|
||||||
|
switch(mode){
|
||||||
|
case GO:
|
||||||
|
analogWrite(Motor::_pin1, speed);
|
||||||
|
digitalWrite(Motor::_pin2, LOW);
|
||||||
|
break;
|
||||||
|
case BACK:
|
||||||
|
analogWrite(Motor::_pin1, ~speed);
|
||||||
|
digitalWrite(Motor::_pin2, HIGH);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
digitalWrite(Motor::_pin1, (mode>>0)&1);
|
||||||
|
digitalWrite(Motor::_pin2, (mode>>1)&1);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Motor::speed(int speed){
|
||||||
|
if(speed<0) Motor::mode(BACK, -speed);
|
||||||
|
else Motor::mode(GO, speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Motor::attach(byte pin1, byte pin2){
|
||||||
|
Motor::_pin1 = pin1;
|
||||||
|
Motor::_pin2 = pin2;
|
||||||
|
pinMode(Motor::_pin1, OUTPUT);
|
||||||
|
pinMode(Motor::_pin2, OUTPUT);
|
||||||
|
digitalWrite(Motor::_pin1, LOW);
|
||||||
|
digitalWrite(Motor::_pin2, LOW);
|
||||||
|
}
|
||||||
31
arduino/catchrobo2012/Motor.h
Normal file
31
arduino/catchrobo2012/Motor.h
Normal file
|
|
@ -0,0 +1,31 @@
|
||||||
|
/*
|
||||||
|
Motor.h
|
||||||
|
(C)2012 kou029w - MIT License [http://kou029w.appspot.com/mit-license.txt]
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef Motor_h
|
||||||
|
#define Motor_h
|
||||||
|
|
||||||
|
#define STOP 0
|
||||||
|
#define GO 1
|
||||||
|
#define BACK 2
|
||||||
|
#define BRAKE 3
|
||||||
|
|
||||||
|
#if defined(ARDUINO) && ARDUINO >= 100
|
||||||
|
#include <Arduino.h>
|
||||||
|
#else
|
||||||
|
#include <WProgram.h>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
class Motor{
|
||||||
|
public:
|
||||||
|
void mode(byte mode);
|
||||||
|
void mode(byte mode, byte speed);
|
||||||
|
void speed(int speed);
|
||||||
|
void attach(byte pin1, byte pin2);
|
||||||
|
private:
|
||||||
|
byte _pin1;
|
||||||
|
byte _pin2;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -1,132 +0,0 @@
|
||||||
#include <Servo.h>
|
|
||||||
#include <Wire.h>
|
|
||||||
#include <WiiNun.h>
|
|
||||||
|
|
||||||
#define PIN_LED 13
|
|
||||||
|
|
||||||
#define PIN_MOTOR_HORIZON_1 5
|
|
||||||
#define PIN_MOTOR_HORIZON_2 4
|
|
||||||
#define PIN_MOTOR_VERTICAL_1 6
|
|
||||||
#define PIN_MOTOR_VERTICAL_2 7
|
|
||||||
|
|
||||||
#define PIN_SERVO_BASE 9
|
|
||||||
#define PIN_SERVO_HAND_1 10
|
|
||||||
#define PIN_SERVO_HAND_2 11
|
|
||||||
#define PIN_SERVO_HAND_3 12
|
|
||||||
|
|
||||||
#define STOP 0
|
|
||||||
#define GO 1
|
|
||||||
#define BACK 2
|
|
||||||
#define BRAKE 3
|
|
||||||
|
|
||||||
#define PIN_PWR A3
|
|
||||||
#define PIN_GND A2
|
|
||||||
|
|
||||||
int loop_cnt = 0;
|
|
||||||
Servo base;
|
|
||||||
Servo hand1;
|
|
||||||
Servo hand2;
|
|
||||||
Servo hand3;
|
|
||||||
WiiNun WiiNun;
|
|
||||||
|
|
||||||
// 台座の角度
|
|
||||||
byte theta = 90; // degree
|
|
||||||
|
|
||||||
void hMotorSpeed(char sspeed){
|
|
||||||
byte speed = abs(sspeed)*2;
|
|
||||||
if(sspeed<0){
|
|
||||||
analogWrite(PIN_MOTOR_HORIZON_1, 0xFF-speed);
|
|
||||||
digitalWrite(PIN_MOTOR_HORIZON_2, HIGH);
|
|
||||||
}else{
|
|
||||||
analogWrite(PIN_MOTOR_HORIZON_1, speed);
|
|
||||||
digitalWrite(PIN_MOTOR_HORIZON_2, LOW);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void vMotorSpeed(char sspeed){
|
|
||||||
byte speed = abs(sspeed)*2;
|
|
||||||
if(sspeed<0){
|
|
||||||
analogWrite(PIN_MOTOR_VERTICAL_1, 0xFF-speed);
|
|
||||||
digitalWrite(PIN_MOTOR_VERTICAL_2, HIGH);
|
|
||||||
}else{
|
|
||||||
analogWrite(PIN_MOTOR_VERTICAL_1, speed);
|
|
||||||
digitalWrite(PIN_MOTOR_VERTICAL_2, LOW);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// 0-2piで返すatan
|
|
||||||
double atan(double x, double y){
|
|
||||||
if(x>=0 && y<0) return atan(y/x)+2*PI;
|
|
||||||
else if(x<0) return atan(y/x)+PI;
|
|
||||||
else return atan(y/x);
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup(){
|
|
||||||
pinMode(PIN_PWR, OUTPUT);
|
|
||||||
pinMode(PIN_GND, OUTPUT);
|
|
||||||
digitalWrite(PIN_PWR, HIGH);
|
|
||||||
digitalWrite(PIN_GND, LOW);
|
|
||||||
delay(100);
|
|
||||||
WiiNun.begin();
|
|
||||||
base.attach(PIN_SERVO_BASE);
|
|
||||||
hand1.attach(PIN_SERVO_HAND_1);
|
|
||||||
hand2.attach(PIN_SERVO_HAND_2);
|
|
||||||
hand3.attach(PIN_SERVO_HAND_3);
|
|
||||||
// Serial.begin(19200);
|
|
||||||
pinMode(PIN_LED , OUTPUT);
|
|
||||||
pinMode(PIN_MOTOR_HORIZON_1 , OUTPUT);
|
|
||||||
pinMode(PIN_MOTOR_HORIZON_2 , OUTPUT);
|
|
||||||
pinMode(PIN_MOTOR_VERTICAL_1, OUTPUT);
|
|
||||||
pinMode(PIN_MOTOR_VERTICAL_2, OUTPUT);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop(){
|
|
||||||
WiiNun.get();
|
|
||||||
byte joyx = WiiNun.data[0];
|
|
||||||
byte joyy = WiiNun.data[1];
|
|
||||||
byte accx = WiiNun.data[2];
|
|
||||||
byte accy = WiiNun.data[3];
|
|
||||||
byte accz = WiiNun.data[4];
|
|
||||||
byte zbut = ((~WiiNun.data[5] >> 0) & 1);
|
|
||||||
byte cbut = ((~WiiNun.data[5] >> 1) & 1);
|
|
||||||
if ((WiiNun.data[5] >> 2) & 1) accx += 2;
|
|
||||||
if ((WiiNun.data[5] >> 3) & 1) accx += 1;
|
|
||||||
if ((WiiNun.data[5] >> 4) & 1) accy += 2;
|
|
||||||
if ((WiiNun.data[5] >> 5) & 1) accy += 1;
|
|
||||||
if ((WiiNun.data[5] >> 6) & 1) accz += 2;
|
|
||||||
if ((WiiNun.data[5] >> 7) & 1) accz += 1;
|
|
||||||
char x = (char)(joyx-0x7F);
|
|
||||||
char y = (char)(joyy-0x7F);
|
|
||||||
char ay = (char)(accy-0x7F);
|
|
||||||
digitalWrite(PIN_LED, LOW);
|
|
||||||
if(zbut){
|
|
||||||
digitalWrite(PIN_LED, HIGH);
|
|
||||||
if(y>0x1F) theta = atan(x,y)*180/PI;
|
|
||||||
else theta -= x/32;
|
|
||||||
if(theta < 10) theta = 10;
|
|
||||||
else if(theta > 170) theta = 170;
|
|
||||||
base.write(theta);
|
|
||||||
}else{
|
|
||||||
hMotorSpeed(y);
|
|
||||||
}
|
|
||||||
vMotorSpeed(ay);
|
|
||||||
|
|
||||||
byte catchDegree = 90;
|
|
||||||
if(cbut){
|
|
||||||
catchDegree = 30;
|
|
||||||
}
|
|
||||||
hand1.write(catchDegree);
|
|
||||||
hand2.write(catchDegree);
|
|
||||||
hand3.write(catchDegree);
|
|
||||||
|
|
||||||
// Serial.print("theta: "); Serial.print(theta);
|
|
||||||
// Serial.print("\tjoyx: "); Serial.print((byte)joyx,DEC);
|
|
||||||
// Serial.print("\tjoyy: "); Serial.print((byte)joyy,DEC);
|
|
||||||
// Serial.print("\taccx: "); Serial.print((byte)accx,DEC);
|
|
||||||
// Serial.print("\taccy: "); Serial.print((byte)accy,DEC);
|
|
||||||
// Serial.print("\taccz: "); Serial.print((byte)accz,DEC);
|
|
||||||
// Serial.print("\tzbut: "); Serial.print((byte)zbut,DEC);
|
|
||||||
// Serial.print("\tcbut: "); Serial.println((byte)cbut,DEC);
|
|
||||||
|
|
||||||
delay(10);
|
|
||||||
}
|
|
||||||
89
arduino/catchrobo2012/catchrobo2012.ino
Normal file
89
arduino/catchrobo2012/catchrobo2012.ino
Normal file
|
|
@ -0,0 +1,89 @@
|
||||||
|
#include <Servo.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <WiiNun.h>
|
||||||
|
#include "Motor.h"
|
||||||
|
|
||||||
|
#define PIN_LED 13
|
||||||
|
|
||||||
|
#define PIN_MOTOR_HORIZON_1 5
|
||||||
|
#define PIN_MOTOR_HORIZON_2 4
|
||||||
|
#define PIN_MOTOR_VERTICAL_1 6
|
||||||
|
#define PIN_MOTOR_VERTICAL_2 7
|
||||||
|
|
||||||
|
#define PIN_SERVO_BASE 9
|
||||||
|
#define PIN_SERVO_CATCHER 10
|
||||||
|
|
||||||
|
#define PIN_PWR A3
|
||||||
|
#define PIN_GND A2
|
||||||
|
|
||||||
|
int loop_cnt = 0;
|
||||||
|
Servo base;
|
||||||
|
Servo catcher;
|
||||||
|
WiiNun wiiNun;
|
||||||
|
Motor horMotor;
|
||||||
|
Motor verMotor;
|
||||||
|
|
||||||
|
byte baseDeg = 90;
|
||||||
|
byte catcherDeg = 90;
|
||||||
|
|
||||||
|
void setup(){
|
||||||
|
pinMode(PIN_PWR, OUTPUT);
|
||||||
|
pinMode(PIN_GND, OUTPUT);
|
||||||
|
digitalWrite(PIN_PWR, HIGH);
|
||||||
|
digitalWrite(PIN_GND, LOW);
|
||||||
|
delay(100);
|
||||||
|
Serial.begin(19200);
|
||||||
|
wiiNun.begin();
|
||||||
|
base.attach(PIN_SERVO_BASE);
|
||||||
|
catcher.attach(PIN_SERVO_CATCHER);
|
||||||
|
pinMode(PIN_LED , OUTPUT);
|
||||||
|
horMotor.attach(PIN_MOTOR_HORIZON_1, PIN_MOTOR_HORIZON_2);
|
||||||
|
verMotor.attach(PIN_MOTOR_VERTICAL_1, PIN_MOTOR_VERTICAL_2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop(){
|
||||||
|
wiiNun.get();
|
||||||
|
byte joyx = wiiNun.data[0];
|
||||||
|
byte joyy = wiiNun.data[1];
|
||||||
|
byte accx = wiiNun.data[2];
|
||||||
|
byte accy = wiiNun.data[3];
|
||||||
|
byte accz = wiiNun.data[4];
|
||||||
|
byte zbut = ((~wiiNun.data[5] >> 0) & 1);
|
||||||
|
byte cbut = ((~wiiNun.data[5] >> 1) & 1);
|
||||||
|
if ((wiiNun.data[5] >> 2) & 1) accx += 2;
|
||||||
|
if ((wiiNun.data[5] >> 3) & 1) accx += 1;
|
||||||
|
if ((wiiNun.data[5] >> 4) & 1) accy += 2;
|
||||||
|
if ((wiiNun.data[5] >> 5) & 1) accy += 1;
|
||||||
|
if ((wiiNun.data[5] >> 6) & 1) accz += 2;
|
||||||
|
if ((wiiNun.data[5] >> 7) & 1) accz += 1;
|
||||||
|
|
||||||
|
char x = (char)(joyx-0x7F);
|
||||||
|
char y = (char)(joyy-0x7F);
|
||||||
|
// char ax = (char)(accx-0x7F);
|
||||||
|
char ay = (char)(accy-0x7F);
|
||||||
|
|
||||||
|
digitalWrite(PIN_LED, LOW);
|
||||||
|
if(zbut){ // zボタン押しながら
|
||||||
|
digitalWrite(PIN_LED, HIGH);
|
||||||
|
if(y>0x1F) baseDeg = 180 - atan2(y, x)*180/PI; // 台座をスティックの角度にする
|
||||||
|
if(baseDeg < 10) baseDeg = 10;
|
||||||
|
else if(baseDeg > 170) baseDeg = 170;
|
||||||
|
}else{
|
||||||
|
horMotor.speed(y);
|
||||||
|
baseDeg += x/32; // 台座をxに応じて相対移動
|
||||||
|
}
|
||||||
|
|
||||||
|
if(ay > +0x10) verMotor.speed(+0xCC);
|
||||||
|
if(ay < -0x10) verMotor.speed(-0xCC);
|
||||||
|
if(ay > +0x20) verMotor.mode(GO);
|
||||||
|
if(ay < -0x20) verMotor.mode(BACK);
|
||||||
|
|
||||||
|
catcherDeg = 90;
|
||||||
|
if(cbut){
|
||||||
|
catcherDeg = 15;
|
||||||
|
}
|
||||||
|
|
||||||
|
base.attach(baseDeg);
|
||||||
|
catcher.write(catcherDeg);
|
||||||
|
delay(10);
|
||||||
|
}
|
||||||
Loading…
Add table
Reference in a new issue