Diff: main.cpp

Revision:

0:7d81654c5ce8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 24 07:51:58 2019 +0000
@@ -0,0 +1,471 @@
+//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);
+}
\ No newline at end of file