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