古い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