File content as of revision 0:7d81654c5ce8:

//LEFT ARM
#include "mbed.h"
#include "WiiClassicController.h"
#include "lancer_speaker.h"
#include "SerialServo.h"

#define PI  3.14159265
#define CONTROL_PERIOD_MS 10

#define L1 440  //第一腕長(mm)
#define L2 350  //第二腕長(mm)

#define MAX_LEN 770  //最大展開長
#define MIN_LEN 150  //最小縮小長

#define RC_CENTER 1500 //RCサーボ中心値(us)
#define RC_MAX 2200  //RCサーボ最大値
#define RC_MIN 850  //RCサーボ最小値

#define SIGN    1 //左アームは1,右アームは-1

//初期位置
#define X_HOME 200
#define Y_HOME 550
#define Z_HOME 0

#define SERVO1_ZERO   415 //アーム第1関節零点
#define SERVO2_ZERO   598 //アーム第2関節零点

#define LX_CENTER 32  //クラコン左スティック左右中心値
#define LY_CENTER 32  //クラコン左スティック上下中心値

#define RY_CENTER 15  //右スティック中心値

#define LX_MARGIN 2
#define LY_MARGIN 2

#define FAST 0.2
#define SLOW 0.15

#define ADD_TEMP 0.1

RawSerial pc(USBTX, USBRX);
DigitalIn sens_u(p17);
DigitalIn sens_d(p15);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut pump(p25);   //真空ポンプ
DigitalOut valve(p26);  //電磁弁
DigitalOut stby(p22);
PwmOut motor(p21);

Speaker speaker(p30);

SerialServo servo2(p13,p14);
SerialServo servo1(p9,p10);
WiiClassicController wii(p28, p27); // SDA , SCL
DigitalOut rc_servo(p24);
Ticker soft_pwm;
Timer timer;

void generatePwm_us(int, int);
int ang_c(double x, double y, int sign);

bool isStart = false;  //競技開始
bool isVacuum = false;  //真空ポンプ
bool isRelease = false;  //負圧解放
bool isDash = false;     //ダッシュモード(x, y)
bool isZ_lLimit = false;  //z軸高さ限界
bool isZ_uLimit = false;

int z_nout_time = 0;

double ang1;    //根元角度(rad)
double ang2;    //関節角度(rad)

float xy_speed = SLOW;

int x_point;  //各座標
int y_point;
int z_point;

int rc_angle;  //RCサーボ角度
float angle_offset;
float motor_power;  //上下モータ出力

float angle;
int pre_angle;

int home;
int minus;
int plus;
int bL;
int bR;
int bZL;
int bZR;
int bA;
int bB;
int bX;
int bY;
int bDU;
int bDD;
int bDL;
int bDR;

typedef union{
    short i;
    char b[2];
}short_b;

int init();
int check();
int output();
void generate_pwm_us(int, int); 
void beep_A();
void beep_B();

int main() {
    int count=0;
    init();
    while(1) {
    check();
    output();
    while(timer.read_ms() < CONTROL_PERIOD_MS);
    //pc.printf("%d:%f\n", count, timer.read());
    timer.reset();
    count++;
    }
}

int init()
{
    short s;
    char c;
    timer.start();
    motor.period_us(80);
    pc.baud(9600);
    servo1.start(38400, 128);
    servo2.start(38400, 128);
    generatePwm_us(1500, 20000);
    stby = 1; 
    x_point = X_HOME;
    y_point = Y_HOME;
    
    speaker.tone(415, 100);
    wait_ms(120);
    speaker.tone(659, 100);
    wait_ms(120);
    speaker.tone(831, 100);
    wait_ms(120);
    speaker.tone(1046, 100);
    wait_ms(500);
    
    return 0;
}

int check()
{  
    home = wii.button_home();
    minus = wii.button_minus();
    plus = wii.button_plus();
    bL = wii.button_L();
    bR = wii.button_R();
    bZL = wii.button_ZL();
    bZR = wii.button_ZR();
    bA = wii.button_A();
    bB = wii.button_B();
    bX = wii.button_X();
    bY = wii.button_Y();
    bDU = wii.button_DU();
    bDD = wii.button_DD();
    bDL = wii.button_DL();
    bDR = wii.button_DR();
    
    static int pre_home = home;
    static int pre_minus = minus;
    static int pre_plus = plus;
    static int pre_bL = bL;
    static int pre_bR = bR;
    static int pre_bZL = bZL;
    static int pre_bZR = bZR;
    static int pre_bA = bA;
    static int pre_bB = bB;
    static int pre_bX = bX;
    static int pre_bY = bY;
    static int pre_bDU = bDU;
    static int pre_bDD = bDD;
    static int pre_bDL = bDL;
    static int pre_bDR = bDR;
    static int pre_x = x_point;
    static int pre_y = y_point;
    static int pre_z = z_point;

    char c;
    short s;
/*
    servo1.sendShort('G', 0);
    if(servo1.readShort(&c, &s)==0){
        if(c=='G'){  led1=!led1;pc.printf("SERVO1=%d\t", s);}
        else led1=0;
    }

    servo2.sendShort('G',0);
    if(servo2.readShort(&c, &s)==0){
        if(c=='G'){  led2=!led2;pc.printf("SERVO2=%d\n", s);}
        else led2=0;
    }
*/
    if(bR == 1)  angle+=15;
    else if(bL == 1)  angle-=15;
  
    
    int joyLX = wii.joy_LX() - LX_CENTER;
    int joyLY = wii.joy_LY() - LY_CENTER;
    
    if(joyLX < -LX_MARGIN || joyLX > LX_MARGIN)
        x_point = x_point - (float)joyLX*(float)xy_speed; 
        
    if(joyLY < -LY_MARGIN || joyLY > LY_MARGIN)
        y_point = y_point - (float)joyLY*(float)xy_speed;
    
    //pc.printf("x_point=%d\ty_point=%d\nxy_speed=%f", x_point, y_point, xy_speed);
    pc.printf("u=%d\td=%d\n", sens_u.read(), sens_d.read());
/*   
    if(x_point < -500){
        x_point = -500;
        beep_B();
    }else if(x_point > 700){
        x_point = 700;
        beep_B();
    }
    
    if(y_point > 560){
        y_point = 560;
        beep_B();   
    }else if(y_point < -500){
        y_point = -500;
        beep_B();
    }
    
    if(x_point < 160 && y_point < 160){
        x_point = pre_x;
        y_point = pre_y;
        beep_B();
    } 
    */
    
    //MFTarea
    if(x_point < -400){
        x_point = -400;
        beep_B();
    }else if(x_point > 600){
        x_point = 600;
        beep_B();
    }
    
    if(y_point < 500){
        y_point = 500;
        beep_B();
    }else if(y_point > 770){
        y_point = 770;
        beep_B();
    }
    
    
    if(bX == 1 && pre_bX == 0)   isVacuum = !isVacuum;
    
    if(bZL == 1 && pre_bZL == 0)   isDash = !isDash;
    
    if(isDash == true){
        if(xy_speed <= FAST)    xy_speed += ADD_TEMP;
    }
    else{
        if(xy_speed > SLOW)    xy_speed -= ADD_TEMP;
    }
    
    if(bZR == 1 )   isRelease = 1;
    else            isRelease = 0;
    
    /*
    if(sens_u)  isZ_uLimit = false;
    else        isZ_uLimit = true;
    
    if(sens_d)  isZ_lLimit = false;
    else        isZ_lLimit = true;
    */
    //pc.printf("sens_u=%d\tsens_d=%d\n", isZ_uLimit, isZ_lLimit); 
    
    int joyRY = wii.joy_RY() - RY_CENTER;
    
    //pc.printf("joyRY=%d\n", joyRY);
    
    if(wii.joy_LY() > 60 && wii.joy_LX() > 60){
        x_point = pre_x;
        y_point = pre_y;
        motor_power = 0.5;
        beep_A();
    }else   motor_power = -(double)joyRY/100.0 + 0.5; //motor_power = (double)joyRY/150.0 + 0.5;
    if(isZ_uLimit && (motor_power > 0.5)){
        motor_power = 0.5;
    }else if(isZ_lLimit && (motor_power < 0.5))    motor_power = 0.5;
        
    /*
    pc.printf("----------------\n");
    pc.printf("LX:%d\tLY:%d\n", wii.joy_LX(), wii.joy_LY());
    pc.printf("RX:%d\tRY:%d\n", wii.joy_RX(), wii.joy_RY());
    pc.printf("home:%d\t-:%d\t+:%d\n", wii.button_home(), wii.button_minus(), wii.button_plus());
    pc.printf("L:%d\tR:%d\tZL:%d\tZR:%d\n", wii.button_L(), wii.button_R(), wii.button_ZL(), wii.button_ZR());
    pc.printf("A:%d\tB:%d\tX:%d\tY:%d\n", wii.button_A(), wii.button_B(), wii.button_X(), wii.button_Y()); 
    pc.printf("DU:%d\tDD:%d\tDL:%d\tDR:%d\n", wii.button_DU(), wii.button_DD(), wii.button_DL(), wii.button_DR()); 
    */
    
    //ang1=PI;
    //ang2=0.0;
  
    if(ang_c(x_point,y_point,SIGN)==-1){
        x_point = pre_x;
        y_point = pre_y;
        beep_B();
    }
    
    pre_home = home;
    pre_minus = minus;
    pre_plus = plus;
    pre_bL = bL;
    pre_bR = bR;
    pre_bZL = bZL;
    pre_bZR = bZR;
    pre_bA = bA;
    pre_bB = bB;
    pre_bX = bX;
    pre_bY = bY;
    pre_bDU = bDU;
    pre_bDD = bDD;
    pre_bDL = bDL;
    pre_bDR = bDR;
    pre_x = x_point;
    pre_y = y_point;

    return 0;
}

int output()
{
    char c;
    short s;
    int angle1 = ang1*442.0/PI + SERVO1_ZERO;
    int angle2 = -ang2*460.0/PI + SERVO2_ZERO;
    //int angle1 = SERVO1_ZERO;
    //int angle2 = SERVO2_ZERO;
    //int angle1 = ang1;
    //int angle2 = ang2;
    static int pre_angle1;
    static int pre_angle2;    
    
    //double ang1a = (angle1-SERVO1_ZERO)*PI/442.0;
    //double ang2a = (angle2-SERVO2_ZERO)*PI/409.0;
    
    angle_offset = (float)(ang1 + ang2)*(float)1500.0/180.0;
    
    int x= L1*cos(ang1) + L2*cos(ang1+ang2);
    int y= L1*sin(ang1) + L2*sin(ang1+ang2);
    
    //pc.printf("x=%d\txp=%d\ny=%d\typ=%d\n", x, x_point, y, y_point);
    //pc.printf("angle=%f\n", angle);
    //pc.printf("angle1=%d\tangle2=%d\n", angle1, angle2);
    if(angle1 != pre_angle1){
        servo1.sendShort('S', angle1);
        if(servo1.readShort(&c, &s)==0){
            led1=!led1;
            if(c=='S'&&s==-1)    angle1 = pre_angle1;
        }
        pre_angle1 = angle1;
    }
    
    if(angle2 != pre_angle2){
        servo2.sendShort('S', angle2);
        if(servo2.readShort(&c, &s)==0){
            led2 = !led2;
            if(c=='S'&&s==-1)    angle2 = pre_angle2;
        }
        pre_angle2 = angle2;
    } 
    
    int angle_output = RC_CENTER + angle; //+ angle_offset;
    
    if(angle_output > RC_MAX){
           angle_output = RC_MAX;
           angle = RC_MAX  - RC_CENTER;
    }else if(angle_output < RC_MIN){
          angle_output = RC_MIN;
          angle = RC_MIN - RC_CENTER;
    }
    generatePwm_us(RC_CENTER + angle - angle_offset, 20000);
    
    if(isVacuum == 1)   pump = 1;
    else                pump = 0;
    
    //pc.printf("pump=%d\n", isVacuum);
    
    if(isRelease == 1)   valve = 1;
    else                valve = 0;
    
    //pc.printf("motor_p = %f\n", motor_power);
    
    if(motor_power == 0.5)  z_nout_time++;
    else z_nout_time=0;
    
    if(motor_power == 0.5 && ((z_nout_time*CONTROL_PERIOD_MS) > 300)){
        stby = 0;
    }else{
        stby = 1;
        motor = motor_power;
    }
    return 0;
}

//角度計算
int ang_c(double x, double y,int sign)
{
    static double pre_ang1=ang1;
    static double pre_ang2=ang2;
    ang2 = sign*acos((x*x + y*y - L1*L1 - L2*L2)/(2.0*L1*L2));
    ang1 = atan(y/x) - atan((L2*sin(ang2))/(L1+L2*cos(ang2)));
    if(x<0) ang1=ang1+PI;
    if(ang1 > PI)   ang1 -= 2*PI;
    if(ang2 > PI)   ang2 -= 2*PI;

    if(sqrt(x*x+y*y) > MAX_LEN || sqrt(x*x+y*y) <= MIN_LEN){
        ang1 = pre_ang1;
        ang2 = pre_ang2;
        return -1;
    }
    pre_ang1 = ang1;
    pre_ang2 = ang2;
    return 0;
}

//SOFT PWM SERVO

#define SOFT_PWM_CYCLE 15

int soft_pwm_period=0;
int soft_pulse_width=0;

void generatePWM(){
   static int count = 0;
   if ((count++)*SOFT_PWM_CYCLE < soft_pulse_width)
      rc_servo = 1;
   else
      rc_servo = 0;
   if (count*SOFT_PWM_CYCLE > soft_pwm_period)
      count = 0;
}

void generatePwm_us(int p_width, int p_period)
{
    if(soft_pwm_period != p_period){
        soft_pwm_period = p_period;
        soft_pulse_width = p_width;
        soft_pwm.attach_us(&generatePWM, SOFT_PWM_CYCLE);
    }else   soft_pulse_width = p_width;
}

void beep_A(){
    speaker.tone(2000, 300);
}

void beep_B(){
    speaker.tone(1000, 300);
}