古いcatchroboを削除
This commit is contained in:
parent
1e3dc6fc15
commit
8639e2d3f8
1 changed files with 0 additions and 181 deletions
|
|
@ -1,181 +0,0 @@
|
||||||
#include <Servo.h>
|
|
||||||
#include <Wire.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 pwrpin PORTC3
|
|
||||||
#define gndpin PORTC2
|
|
||||||
|
|
||||||
byte nunchuck_buf[6];
|
|
||||||
int loop_cnt = 0;
|
|
||||||
Servo base;
|
|
||||||
Servo hand1;
|
|
||||||
Servo hand2;
|
|
||||||
Servo hand3;
|
|
||||||
|
|
||||||
// 台座の角度
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void nunchuckInit(){
|
|
||||||
DDRC |= _BV(pwrpin) | _BV(gndpin);
|
|
||||||
PORTC &=~ _BV(gndpin);
|
|
||||||
PORTC |= _BV(pwrpin);
|
|
||||||
delay(100);
|
|
||||||
Wire.begin();
|
|
||||||
Wire.beginTransmission(0x52);
|
|
||||||
Wire.write(0x40);// sends memory address
|
|
||||||
Wire.write(0x00);// sends sent a zero
|
|
||||||
Wire.endTransmission();// stop transmitting
|
|
||||||
}
|
|
||||||
|
|
||||||
// Send a request for data to the nunchuck
|
|
||||||
// was "send_zero()"
|
|
||||||
void nunchuck_send_request()
|
|
||||||
{
|
|
||||||
Wire.beginTransmission(0x52);// transmit to device 0x52
|
|
||||||
Wire.write(0x00);// sends one byte
|
|
||||||
Wire.endTransmission();// stop transmitting
|
|
||||||
}
|
|
||||||
|
|
||||||
// Encode data to format that most wiimote drivers except
|
|
||||||
// only needed if you use one of the regular wiimote drivers
|
|
||||||
char nunchuk_decode_byte (char x)
|
|
||||||
{
|
|
||||||
x = (x ^ 0x17) + 0x17;
|
|
||||||
return x;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Receive data back from the nunchuck,
|
|
||||||
// returns 1 on successful read. returns 0 on failure
|
|
||||||
byte nunchuckGet(){
|
|
||||||
int cnt=0;
|
|
||||||
Wire.requestFrom (0x52, 6);// request data from nunchuck
|
|
||||||
while (Wire.available()) {
|
|
||||||
// receive byte as an integer
|
|
||||||
nunchuck_buf[cnt] = nunchuk_decode_byte(Wire.read());
|
|
||||||
cnt++;
|
|
||||||
}
|
|
||||||
nunchuck_send_request(); // send request for next data payload
|
|
||||||
// If we recieved the 6 bytes, then go print them
|
|
||||||
if (cnt >= 5) {
|
|
||||||
return 1; // success
|
|
||||||
}
|
|
||||||
return 0; //failure
|
|
||||||
}
|
|
||||||
|
|
||||||
// 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(){
|
|
||||||
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);
|
|
||||||
nunchuckInit();
|
|
||||||
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(){
|
|
||||||
nunchuckGet();
|
|
||||||
byte joyx = nunchuck_buf[0];
|
|
||||||
byte joyy = nunchuck_buf[1];
|
|
||||||
byte accx = nunchuck_buf[2];
|
|
||||||
byte accy = nunchuck_buf[3];
|
|
||||||
byte accz = nunchuck_buf[4];
|
|
||||||
byte zbut = ((~nunchuck_buf[5] >> 0) & 1);
|
|
||||||
byte cbut = ((~nunchuck_buf[5] >> 1) & 1);
|
|
||||||
if ((nunchuck_buf[5] >> 2) & 1)
|
|
||||||
accx += 2;
|
|
||||||
if ((nunchuck_buf[5] >> 3) & 1)
|
|
||||||
accx += 1;
|
|
||||||
if ((nunchuck_buf[5] >> 4) & 1)
|
|
||||||
accy += 2;
|
|
||||||
if ((nunchuck_buf[5] >> 5) & 1)
|
|
||||||
accy += 1;
|
|
||||||
if ((nunchuck_buf[5] >> 6) & 1)
|
|
||||||
accz += 2;
|
|
||||||
if ((nunchuck_buf[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);
|
|
||||||
}
|
|
||||||
Loading…
Add table
Reference in a new issue