[延伸範例] 用wii搖桿控制小機械手臂

 小機械手臂

手臂上共有4顆馬達為KONDO的KRS-786馬達 有feedback的功能

而前端的DMP小手之前介紹過 所以這邊不多說



介紹影片:

第一次上鏡頭有點緊張啦...

啟動的時候會震動是因為手臂下方過輕...

原本想要塞一些東西的  不過還沒找到適合的物品

以及剪刀石頭布的布  因為正常狀態下就是布了  所以我沒有多做

附註: DMP小手雖然是三個自由度   但還是有密技可做出奇怪的手勢(請自行想像)



以下是code:

#include "stdio.h"
#include "conio.h"
#define USE_COMMON
#include "roboard.h"
#include "roboard_dll.h"

const unsigned char wii_addr = 0x52;
int joy_x_axis;
int joy_y_axis;
int accel_x_axis;
int accel_y_axis;
int accel_z_axis;
int z_button = 0;
int c_button = 0;
void wii_data(unsigned char*);
void rock(unsigned long);
void paper(unsigned long);
void scissors(unsigned long);
int main(){
 
 unsigned long motion_frame[32] = {1500L};
 unsigned char buf[6];
 unsigned long PWM_period = 10000L;

 roboio_SetRBVer(RB_110);
 rcservo_SetServo(RCSERVO_PINS1, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS2, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS3, RCSERVO_DMP_RS0263);
 rcservo_SetServo(RCSERVO_PINS4, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS5, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS6, RCSERVO_KONDO_KRS78X);
 rcservo_SetServo(RCSERVO_PINS7, RCSERVO_KONDO_KRS78X);
 rcservo_Init(RCSERVO_USEPINS1 + RCSERVO_USEPINS2 + RCSERVO_USEPINS3 + RCSERVO_USEPINS4 +
    RCSERVO_USEPINS5 + RCSERVO_USEPINS6 + RCSERVO_USEPINS7);
    i2c_Initialize(I2CIRQ_DISABLE);
 i2c0_SetSpeed(I2CMODE_FAST, 400000L);
 i2c0master_StartN(wii_addr, I2C_WRITE, 2);
 i2c0master_WriteN(0x40);
 i2c0master_WriteN(0x00);

 rcservo_EnterPlayMode();
 rcservo_SetAction(motion_frame, 1000); 
 while(!kbhit()){

  i2c0master_StartN(wii_addr, I2C_WRITE, 1);
  i2c0master_WriteN(0x00);
  delay_ms(5);
  i2c_Receive(wii_addr,buf,6);
  for(int i = 0; i < 6; i++)
   buf[i] = (buf[i] ^ 0x17) + 0x17;
  wii_data(buf);


  if(z_button && !c_button)
   rock(PWM_period);
  else if(!z_button && c_button)
      scissors(PWM_period);
  else if(!z_button && !c_button)
   paper(PWM_period);
  else if(z_button && c_button){
   printf("Enter sleep mode.\n");
   rcservo_EnterCaptureMode();
   delay_ms(5000);
   printf("You can click C button and Z button to close sleep mode.\n");
   while(1){
    i2c0master_StartN(wii_addr, I2C_WRITE, 1);
    i2c0master_WriteN(0x00);
    delay_ms(5);
    i2c_Receive(wii_addr,buf,6);
    for(int i = 0; i < 6; i++)
     buf[i] = (buf[i] ^ 0x17) + 0x17;
    wii_data(buf);
    if(z_button && c_button){
     printf("Close sleep mode.\n");
     break;
    }
    delay_ms(100);
   }
  }
  motion_frame[3] = ((joy_x_axis - 34) * 8.5) + 600;
  motion_frame[5] = ((joy_y_axis - 25) * 8.8) + 600;
  motion_frame[4] = ((accel_y_axis - 285) * 4.3) + 600;
  motion_frame[6] = 2300 - ((accel_x_axis - 285) * 4.3);
  rcservo_EnterPlayMode();
  rcservo_SetAction(motion_frame, 1000); 
  rcservo_SetFPS(50);
  while(rcservo_PlayAction() != RCSERVO_PLAYEND);
  
 }
 i2c_Close();
 rcservo_Close();
 return 0;
}
void wii_data(unsigned char *buf){
joy_x_axis = buf[0];
joy_y_axis = buf[1];
accel_x_axis = buf[2] << 2;
accel_y_axis = buf[3] << 2;
accel_z_axis = buf[4] << 2;
z_button = 1;
c_button = 1;
if ((buf[5] >> 0) & 1) z_button = 0;
if ((buf[5] >> 1) & 1) c_button = 0;
if ((buf[5] >> 2) & 1) accel_x_axis += 2;
if ((buf[5] >> 3) & 1) accel_x_axis += 1;
if ((buf[5] >> 4) & 1) accel_y_axis += 2;
if ((buf[5] >> 5) & 1) accel_y_axis += 1;
if ((buf[5] >> 6) & 1) accel_z_axis += 2;
if ((buf[5] >> 7) & 1) accel_z_axis += 1;
}
void rock(unsigned long PWM_period){
 unsigned long PINS1_duty = 1800L;
 unsigned long PINS2_duty = 700L;
 unsigned long PINS3_duty = 1800L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}
void paper(unsigned long PWM_period){
 unsigned long PINS1_duty = 700L;
 unsigned long PINS2_duty = 1800L;
 unsigned long PINS3_duty = 700L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}
void scissors(unsigned long PWM_period){
 unsigned long PINS1_duty = 1800L;
 unsigned long PINS2_duty = 1800L;
 unsigned long PINS3_duty = 1800L;
 rcservo_SendCPWM(RCSERVO_PINS1, PWM_period, PINS1_duty);
 rcservo_SendCPWM(RCSERVO_PINS2, PWM_period, PINS2_duty);
 rcservo_SendCPWM(RCSERVO_PINS3, PWM_period, PINS3_duty);
}