diff --git a/arduino/catchrobo/catchrobo.ino b/arduino/catchrobo/catchrobo.ino deleted file mode 100755 index 60140e6..0000000 --- a/arduino/catchrobo/catchrobo.ino +++ /dev/null @@ -1,181 +0,0 @@ -#include -#include - -#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); -}