takahiro kambara / Mbed 2 deprecated 2016_catch_robo_left

2016 catch robo

Dependencies:   mbed WiiClassicController_kai

main.cpp@0:7d81654c5ce8, 2019-06-24 (annotated)

Committer:
kambara1415
Date:
Mon Jun 24 07:51:58 2019 +0000
Revision:
0:7d81654c5ce8
2016 suiheitakannsetu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kambara1415 0:7d81654c5ce8 1 //LEFT ARM
kambara1415 0:7d81654c5ce8 2 #include "mbed.h"
kambara1415 0:7d81654c5ce8 3 #include "WiiClassicController.h"
kambara1415 0:7d81654c5ce8 4 #include "lancer_speaker.h"
kambara1415 0:7d81654c5ce8 5 #include "SerialServo.h"
kambara1415 0:7d81654c5ce8 6
kambara1415 0:7d81654c5ce8 7 #define PI 3.14159265
kambara1415 0:7d81654c5ce8 8 #define CONTROL_PERIOD_MS 10
kambara1415 0:7d81654c5ce8 9
kambara1415 0:7d81654c5ce8 10 #define L1 440 //第一腕長(mm)
kambara1415 0:7d81654c5ce8 11 #define L2 350 //第二腕長(mm)
kambara1415 0:7d81654c5ce8 12
kambara1415 0:7d81654c5ce8 13 #define MAX_LEN 770 //最大展開長
kambara1415 0:7d81654c5ce8 14 #define MIN_LEN 150 //最小縮小長
kambara1415 0:7d81654c5ce8 15
kambara1415 0:7d81654c5ce8 16 #define RC_CENTER 1500 //RCサーボ中心値(us)
kambara1415 0:7d81654c5ce8 17 #define RC_MAX 2200 //RCサーボ最大値
kambara1415 0:7d81654c5ce8 18 #define RC_MIN 850 //RCサーボ最小値
kambara1415 0:7d81654c5ce8 19
kambara1415 0:7d81654c5ce8 20 #define SIGN 1 //左アームは1,右アームは-1
kambara1415 0:7d81654c5ce8 21
kambara1415 0:7d81654c5ce8 22 //初期位置
kambara1415 0:7d81654c5ce8 23 #define X_HOME 200
kambara1415 0:7d81654c5ce8 24 #define Y_HOME 550
kambara1415 0:7d81654c5ce8 25 #define Z_HOME 0
kambara1415 0:7d81654c5ce8 26
kambara1415 0:7d81654c5ce8 27 #define SERVO1_ZERO 415 //アーム第1関節零点
kambara1415 0:7d81654c5ce8 28 #define SERVO2_ZERO 598 //アーム第2関節零点
kambara1415 0:7d81654c5ce8 29
kambara1415 0:7d81654c5ce8 30 #define LX_CENTER 32 //クラコン左スティック左右中心値
kambara1415 0:7d81654c5ce8 31 #define LY_CENTER 32 //クラコン左スティック上下中心値
kambara1415 0:7d81654c5ce8 32
kambara1415 0:7d81654c5ce8 33 #define RY_CENTER 15 //右スティック中心値
kambara1415 0:7d81654c5ce8 34
kambara1415 0:7d81654c5ce8 35 #define LX_MARGIN 2
kambara1415 0:7d81654c5ce8 36 #define LY_MARGIN 2
kambara1415 0:7d81654c5ce8 37
kambara1415 0:7d81654c5ce8 38 #define FAST 0.2
kambara1415 0:7d81654c5ce8 39 #define SLOW 0.15
kambara1415 0:7d81654c5ce8 40
kambara1415 0:7d81654c5ce8 41 #define ADD_TEMP 0.1
kambara1415 0:7d81654c5ce8 42
kambara1415 0:7d81654c5ce8 43 RawSerial pc(USBTX, USBRX);
kambara1415 0:7d81654c5ce8 44 DigitalIn sens_u(p17);
kambara1415 0:7d81654c5ce8 45 DigitalIn sens_d(p15);
kambara1415 0:7d81654c5ce8 46 DigitalOut led1(LED1);
kambara1415 0:7d81654c5ce8 47 DigitalOut led2(LED2);
kambara1415 0:7d81654c5ce8 48 DigitalOut pump(p25); //真空ポンプ
kambara1415 0:7d81654c5ce8 49 DigitalOut valve(p26); //電磁弁
kambara1415 0:7d81654c5ce8 50 DigitalOut stby(p22);
kambara1415 0:7d81654c5ce8 51 PwmOut motor(p21);
kambara1415 0:7d81654c5ce8 52
kambara1415 0:7d81654c5ce8 53 Speaker speaker(p30);
kambara1415 0:7d81654c5ce8 54
kambara1415 0:7d81654c5ce8 55 SerialServo servo2(p13,p14);
kambara1415 0:7d81654c5ce8 56 SerialServo servo1(p9,p10);
kambara1415 0:7d81654c5ce8 57 WiiClassicController wii(p28, p27); // SDA , SCL
kambara1415 0:7d81654c5ce8 58 DigitalOut rc_servo(p24);
kambara1415 0:7d81654c5ce8 59 Ticker soft_pwm;
kambara1415 0:7d81654c5ce8 60 Timer timer;
kambara1415 0:7d81654c5ce8 61
kambara1415 0:7d81654c5ce8 62 void generatePwm_us(int, int);
kambara1415 0:7d81654c5ce8 63 int ang_c(double x, double y, int sign);
kambara1415 0:7d81654c5ce8 64
kambara1415 0:7d81654c5ce8 65 bool isStart = false; //競技開始
kambara1415 0:7d81654c5ce8 66 bool isVacuum = false; //真空ポンプ
kambara1415 0:7d81654c5ce8 67 bool isRelease = false; //負圧解放
kambara1415 0:7d81654c5ce8 68 bool isDash = false; //ダッシュモード(x, y)
kambara1415 0:7d81654c5ce8 69 bool isZ_lLimit = false; //z軸高さ限界
kambara1415 0:7d81654c5ce8 70 bool isZ_uLimit = false;
kambara1415 0:7d81654c5ce8 71
kambara1415 0:7d81654c5ce8 72 int z_nout_time = 0;
kambara1415 0:7d81654c5ce8 73
kambara1415 0:7d81654c5ce8 74 double ang1; //根元角度(rad)
kambara1415 0:7d81654c5ce8 75 double ang2; //関節角度(rad)
kambara1415 0:7d81654c5ce8 76
kambara1415 0:7d81654c5ce8 77 float xy_speed = SLOW;
kambara1415 0:7d81654c5ce8 78
kambara1415 0:7d81654c5ce8 79 int x_point; //各座標
kambara1415 0:7d81654c5ce8 80 int y_point;
kambara1415 0:7d81654c5ce8 81 int z_point;
kambara1415 0:7d81654c5ce8 82
kambara1415 0:7d81654c5ce8 83 int rc_angle; //RCサーボ角度
kambara1415 0:7d81654c5ce8 84 float angle_offset;
kambara1415 0:7d81654c5ce8 85 float motor_power; //上下モータ出力
kambara1415 0:7d81654c5ce8 86
kambara1415 0:7d81654c5ce8 87 float angle;
kambara1415 0:7d81654c5ce8 88 int pre_angle;
kambara1415 0:7d81654c5ce8 89
kambara1415 0:7d81654c5ce8 90 int home;
kambara1415 0:7d81654c5ce8 91 int minus;
kambara1415 0:7d81654c5ce8 92 int plus;
kambara1415 0:7d81654c5ce8 93 int bL;
kambara1415 0:7d81654c5ce8 94 int bR;
kambara1415 0:7d81654c5ce8 95 int bZL;
kambara1415 0:7d81654c5ce8 96 int bZR;
kambara1415 0:7d81654c5ce8 97 int bA;
kambara1415 0:7d81654c5ce8 98 int bB;
kambara1415 0:7d81654c5ce8 99 int bX;
kambara1415 0:7d81654c5ce8 100 int bY;
kambara1415 0:7d81654c5ce8 101 int bDU;
kambara1415 0:7d81654c5ce8 102 int bDD;
kambara1415 0:7d81654c5ce8 103 int bDL;
kambara1415 0:7d81654c5ce8 104 int bDR;
kambara1415 0:7d81654c5ce8 105
kambara1415 0:7d81654c5ce8 106 typedef union{
kambara1415 0:7d81654c5ce8 107 short i;
kambara1415 0:7d81654c5ce8 108 char b[2];
kambara1415 0:7d81654c5ce8 109 }short_b;
kambara1415 0:7d81654c5ce8 110
kambara1415 0:7d81654c5ce8 111 int init();
kambara1415 0:7d81654c5ce8 112 int check();
kambara1415 0:7d81654c5ce8 113 int output();
kambara1415 0:7d81654c5ce8 114 void generate_pwm_us(int, int);
kambara1415 0:7d81654c5ce8 115 void beep_A();
kambara1415 0:7d81654c5ce8 116 void beep_B();
kambara1415 0:7d81654c5ce8 117
kambara1415 0:7d81654c5ce8 118 int main() {
kambara1415 0:7d81654c5ce8 119 int count=0;
kambara1415 0:7d81654c5ce8 120 init();
kambara1415 0:7d81654c5ce8 121 while(1) {
kambara1415 0:7d81654c5ce8 122 check();
kambara1415 0:7d81654c5ce8 123 output();
kambara1415 0:7d81654c5ce8 124 while(timer.read_ms() < CONTROL_PERIOD_MS);
kambara1415 0:7d81654c5ce8 125 //pc.printf("%d:%f\n", count, timer.read());
kambara1415 0:7d81654c5ce8 126 timer.reset();
kambara1415 0:7d81654c5ce8 127 count++;
kambara1415 0:7d81654c5ce8 128 }
kambara1415 0:7d81654c5ce8 129 }
kambara1415 0:7d81654c5ce8 130
kambara1415 0:7d81654c5ce8 131 int init()
kambara1415 0:7d81654c5ce8 132 {
kambara1415 0:7d81654c5ce8 133 short s;
kambara1415 0:7d81654c5ce8 134 char c;
kambara1415 0:7d81654c5ce8 135 timer.start();
kambara1415 0:7d81654c5ce8 136 motor.period_us(80);
kambara1415 0:7d81654c5ce8 137 pc.baud(9600);
kambara1415 0:7d81654c5ce8 138 servo1.start(38400, 128);
kambara1415 0:7d81654c5ce8 139 servo2.start(38400, 128);
kambara1415 0:7d81654c5ce8 140 generatePwm_us(1500, 20000);
kambara1415 0:7d81654c5ce8 141 stby = 1;
kambara1415 0:7d81654c5ce8 142 x_point = X_HOME;
kambara1415 0:7d81654c5ce8 143 y_point = Y_HOME;
kambara1415 0:7d81654c5ce8 144
kambara1415 0:7d81654c5ce8 145 speaker.tone(415, 100);
kambara1415 0:7d81654c5ce8 146 wait_ms(120);
kambara1415 0:7d81654c5ce8 147 speaker.tone(659, 100);
kambara1415 0:7d81654c5ce8 148 wait_ms(120);
kambara1415 0:7d81654c5ce8 149 speaker.tone(831, 100);
kambara1415 0:7d81654c5ce8 150 wait_ms(120);
kambara1415 0:7d81654c5ce8 151 speaker.tone(1046, 100);
kambara1415 0:7d81654c5ce8 152 wait_ms(500);
kambara1415 0:7d81654c5ce8 153
kambara1415 0:7d81654c5ce8 154 return 0;
kambara1415 0:7d81654c5ce8 155 }
kambara1415 0:7d81654c5ce8 156
kambara1415 0:7d81654c5ce8 157 int check()
kambara1415 0:7d81654c5ce8 158 {
kambara1415 0:7d81654c5ce8 159 home = wii.button_home();
kambara1415 0:7d81654c5ce8 160 minus = wii.button_minus();
kambara1415 0:7d81654c5ce8 161 plus = wii.button_plus();
kambara1415 0:7d81654c5ce8 162 bL = wii.button_L();
kambara1415 0:7d81654c5ce8 163 bR = wii.button_R();
kambara1415 0:7d81654c5ce8 164 bZL = wii.button_ZL();
kambara1415 0:7d81654c5ce8 165 bZR = wii.button_ZR();
kambara1415 0:7d81654c5ce8 166 bA = wii.button_A();
kambara1415 0:7d81654c5ce8 167 bB = wii.button_B();
kambara1415 0:7d81654c5ce8 168 bX = wii.button_X();
kambara1415 0:7d81654c5ce8 169 bY = wii.button_Y();
kambara1415 0:7d81654c5ce8 170 bDU = wii.button_DU();
kambara1415 0:7d81654c5ce8 171 bDD = wii.button_DD();
kambara1415 0:7d81654c5ce8 172 bDL = wii.button_DL();
kambara1415 0:7d81654c5ce8 173 bDR = wii.button_DR();
kambara1415 0:7d81654c5ce8 174
kambara1415 0:7d81654c5ce8 175 static int pre_home = home;
kambara1415 0:7d81654c5ce8 176 static int pre_minus = minus;
kambara1415 0:7d81654c5ce8 177 static int pre_plus = plus;
kambara1415 0:7d81654c5ce8 178 static int pre_bL = bL;
kambara1415 0:7d81654c5ce8 179 static int pre_bR = bR;
kambara1415 0:7d81654c5ce8 180 static int pre_bZL = bZL;
kambara1415 0:7d81654c5ce8 181 static int pre_bZR = bZR;
kambara1415 0:7d81654c5ce8 182 static int pre_bA = bA;
kambara1415 0:7d81654c5ce8 183 static int pre_bB = bB;
kambara1415 0:7d81654c5ce8 184 static int pre_bX = bX;
kambara1415 0:7d81654c5ce8 185 static int pre_bY = bY;
kambara1415 0:7d81654c5ce8 186 static int pre_bDU = bDU;
kambara1415 0:7d81654c5ce8 187 static int pre_bDD = bDD;
kambara1415 0:7d81654c5ce8 188 static int pre_bDL = bDL;
kambara1415 0:7d81654c5ce8 189 static int pre_bDR = bDR;
kambara1415 0:7d81654c5ce8 190 static int pre_x = x_point;
kambara1415 0:7d81654c5ce8 191 static int pre_y = y_point;
kambara1415 0:7d81654c5ce8 192 static int pre_z = z_point;
kambara1415 0:7d81654c5ce8 193
kambara1415 0:7d81654c5ce8 194 char c;
kambara1415 0:7d81654c5ce8 195 short s;
kambara1415 0:7d81654c5ce8 196 /*
kambara1415 0:7d81654c5ce8 197 servo1.sendShort('G', 0);
kambara1415 0:7d81654c5ce8 198 if(servo1.readShort(&c, &s)==0){
kambara1415 0:7d81654c5ce8 199 if(c=='G'){ led1=!led1;pc.printf("SERVO1=%d\t", s);}
kambara1415 0:7d81654c5ce8 200 else led1=0;
kambara1415 0:7d81654c5ce8 201 }
kambara1415 0:7d81654c5ce8 202
kambara1415 0:7d81654c5ce8 203 servo2.sendShort('G',0);
kambara1415 0:7d81654c5ce8 204 if(servo2.readShort(&c, &s)==0){
kambara1415 0:7d81654c5ce8 205 if(c=='G'){ led2=!led2;pc.printf("SERVO2=%d\n", s);}
kambara1415 0:7d81654c5ce8 206 else led2=0;
kambara1415 0:7d81654c5ce8 207 }
kambara1415 0:7d81654c5ce8 208 */
kambara1415 0:7d81654c5ce8 209 if(bR == 1) angle+=15;
kambara1415 0:7d81654c5ce8 210 else if(bL == 1) angle-=15;
kambara1415 0:7d81654c5ce8 211
kambara1415 0:7d81654c5ce8 212
kambara1415 0:7d81654c5ce8 213 int joyLX = wii.joy_LX() - LX_CENTER;
kambara1415 0:7d81654c5ce8 214 int joyLY = wii.joy_LY() - LY_CENTER;
kambara1415 0:7d81654c5ce8 215
kambara1415 0:7d81654c5ce8 216 if(joyLX < -LX_MARGIN || joyLX > LX_MARGIN)
kambara1415 0:7d81654c5ce8 217 x_point = x_point - (float)joyLX*(float)xy_speed;
kambara1415 0:7d81654c5ce8 218
kambara1415 0:7d81654c5ce8 219 if(joyLY < -LY_MARGIN || joyLY > LY_MARGIN)
kambara1415 0:7d81654c5ce8 220 y_point = y_point - (float)joyLY*(float)xy_speed;
kambara1415 0:7d81654c5ce8 221
kambara1415 0:7d81654c5ce8 222 //pc.printf("x_point=%d\ty_point=%d\nxy_speed=%f", x_point, y_point, xy_speed);
kambara1415 0:7d81654c5ce8 223 pc.printf("u=%d\td=%d\n", sens_u.read(), sens_d.read());
kambara1415 0:7d81654c5ce8 224 /*
kambara1415 0:7d81654c5ce8 225 if(x_point < -500){
kambara1415 0:7d81654c5ce8 226 x_point = -500;
kambara1415 0:7d81654c5ce8 227 beep_B();
kambara1415 0:7d81654c5ce8 228 }else if(x_point > 700){
kambara1415 0:7d81654c5ce8 229 x_point = 700;
kambara1415 0:7d81654c5ce8 230 beep_B();
kambara1415 0:7d81654c5ce8 231 }
kambara1415 0:7d81654c5ce8 232
kambara1415 0:7d81654c5ce8 233 if(y_point > 560){
kambara1415 0:7d81654c5ce8 234 y_point = 560;
kambara1415 0:7d81654c5ce8 235 beep_B();
kambara1415 0:7d81654c5ce8 236 }else if(y_point < -500){
kambara1415 0:7d81654c5ce8 237 y_point = -500;
kambara1415 0:7d81654c5ce8 238 beep_B();
kambara1415 0:7d81654c5ce8 239 }
kambara1415 0:7d81654c5ce8 240
kambara1415 0:7d81654c5ce8 241 if(x_point < 160 && y_point < 160){
kambara1415 0:7d81654c5ce8 242 x_point = pre_x;
kambara1415 0:7d81654c5ce8 243 y_point = pre_y;
kambara1415 0:7d81654c5ce8 244 beep_B();
kambara1415 0:7d81654c5ce8 245 }
kambara1415 0:7d81654c5ce8 246 */
kambara1415 0:7d81654c5ce8 247
kambara1415 0:7d81654c5ce8 248 //MFTarea
kambara1415 0:7d81654c5ce8 249 if(x_point < -400){
kambara1415 0:7d81654c5ce8 250 x_point = -400;
kambara1415 0:7d81654c5ce8 251 beep_B();
kambara1415 0:7d81654c5ce8 252 }else if(x_point > 600){
kambara1415 0:7d81654c5ce8 253 x_point = 600;
kambara1415 0:7d81654c5ce8 254 beep_B();
kambara1415 0:7d81654c5ce8 255 }
kambara1415 0:7d81654c5ce8 256
kambara1415 0:7d81654c5ce8 257 if(y_point < 500){
kambara1415 0:7d81654c5ce8 258 y_point = 500;
kambara1415 0:7d81654c5ce8 259 beep_B();
kambara1415 0:7d81654c5ce8 260 }else if(y_point > 770){
kambara1415 0:7d81654c5ce8 261 y_point = 770;
kambara1415 0:7d81654c5ce8 262 beep_B();
kambara1415 0:7d81654c5ce8 263 }
kambara1415 0:7d81654c5ce8 264
kambara1415 0:7d81654c5ce8 265
kambara1415 0:7d81654c5ce8 266 if(bX == 1 && pre_bX == 0) isVacuum = !isVacuum;
kambara1415 0:7d81654c5ce8 267
kambara1415 0:7d81654c5ce8 268 if(bZL == 1 && pre_bZL == 0) isDash = !isDash;
kambara1415 0:7d81654c5ce8 269
kambara1415 0:7d81654c5ce8 270 if(isDash == true){
kambara1415 0:7d81654c5ce8 271 if(xy_speed <= FAST) xy_speed += ADD_TEMP;
kambara1415 0:7d81654c5ce8 272 }
kambara1415 0:7d81654c5ce8 273 else{
kambara1415 0:7d81654c5ce8 274 if(xy_speed > SLOW) xy_speed -= ADD_TEMP;
kambara1415 0:7d81654c5ce8 275 }
kambara1415 0:7d81654c5ce8 276
kambara1415 0:7d81654c5ce8 277 if(bZR == 1 ) isRelease = 1;
kambara1415 0:7d81654c5ce8 278 else isRelease = 0;
kambara1415 0:7d81654c5ce8 279
kambara1415 0:7d81654c5ce8 280 /*
kambara1415 0:7d81654c5ce8 281 if(sens_u) isZ_uLimit = false;
kambara1415 0:7d81654c5ce8 282 else isZ_uLimit = true;
kambara1415 0:7d81654c5ce8 283
kambara1415 0:7d81654c5ce8 284 if(sens_d) isZ_lLimit = false;
kambara1415 0:7d81654c5ce8 285 else isZ_lLimit = true;
kambara1415 0:7d81654c5ce8 286 */
kambara1415 0:7d81654c5ce8 287 //pc.printf("sens_u=%d\tsens_d=%d\n", isZ_uLimit, isZ_lLimit);
kambara1415 0:7d81654c5ce8 288
kambara1415 0:7d81654c5ce8 289 int joyRY = wii.joy_RY() - RY_CENTER;
kambara1415 0:7d81654c5ce8 290
kambara1415 0:7d81654c5ce8 291 //pc.printf("joyRY=%d\n", joyRY);
kambara1415 0:7d81654c5ce8 292
kambara1415 0:7d81654c5ce8 293 if(wii.joy_LY() > 60 && wii.joy_LX() > 60){
kambara1415 0:7d81654c5ce8 294 x_point = pre_x;
kambara1415 0:7d81654c5ce8 295 y_point = pre_y;
kambara1415 0:7d81654c5ce8 296 motor_power = 0.5;
kambara1415 0:7d81654c5ce8 297 beep_A();
kambara1415 0:7d81654c5ce8 298 }else motor_power = -(double)joyRY/100.0 + 0.5; //motor_power = (double)joyRY/150.0 + 0.5;
kambara1415 0:7d81654c5ce8 299 if(isZ_uLimit && (motor_power > 0.5)){
kambara1415 0:7d81654c5ce8 300 motor_power = 0.5;
kambara1415 0:7d81654c5ce8 301 }else if(isZ_lLimit && (motor_power < 0.5)) motor_power = 0.5;
kambara1415 0:7d81654c5ce8 302
kambara1415 0:7d81654c5ce8 303 /*
kambara1415 0:7d81654c5ce8 304 pc.printf("----------------\n");
kambara1415 0:7d81654c5ce8 305 pc.printf("LX:%d\tLY:%d\n", wii.joy_LX(), wii.joy_LY());
kambara1415 0:7d81654c5ce8 306 pc.printf("RX:%d\tRY:%d\n", wii.joy_RX(), wii.joy_RY());
kambara1415 0:7d81654c5ce8 307 pc.printf("home:%d\t-:%d\t+:%d\n", wii.button_home(), wii.button_minus(), wii.button_plus());
kambara1415 0:7d81654c5ce8 308 pc.printf("L:%d\tR:%d\tZL:%d\tZR:%d\n", wii.button_L(), wii.button_R(), wii.button_ZL(), wii.button_ZR());
kambara1415 0:7d81654c5ce8 309 pc.printf("A:%d\tB:%d\tX:%d\tY:%d\n", wii.button_A(), wii.button_B(), wii.button_X(), wii.button_Y());
kambara1415 0:7d81654c5ce8 310 pc.printf("DU:%d\tDD:%d\tDL:%d\tDR:%d\n", wii.button_DU(), wii.button_DD(), wii.button_DL(), wii.button_DR());
kambara1415 0:7d81654c5ce8 311 */
kambara1415 0:7d81654c5ce8 312
kambara1415 0:7d81654c5ce8 313 //ang1=PI;
kambara1415 0:7d81654c5ce8 314 //ang2=0.0;
kambara1415 0:7d81654c5ce8 315
kambara1415 0:7d81654c5ce8 316 if(ang_c(x_point,y_point,SIGN)==-1){
kambara1415 0:7d81654c5ce8 317 x_point = pre_x;
kambara1415 0:7d81654c5ce8 318 y_point = pre_y;
kambara1415 0:7d81654c5ce8 319 beep_B();
kambara1415 0:7d81654c5ce8 320 }
kambara1415 0:7d81654c5ce8 321
kambara1415 0:7d81654c5ce8 322 pre_home = home;
kambara1415 0:7d81654c5ce8 323 pre_minus = minus;
kambara1415 0:7d81654c5ce8 324 pre_plus = plus;
kambara1415 0:7d81654c5ce8 325 pre_bL = bL;
kambara1415 0:7d81654c5ce8 326 pre_bR = bR;
kambara1415 0:7d81654c5ce8 327 pre_bZL = bZL;
kambara1415 0:7d81654c5ce8 328 pre_bZR = bZR;
kambara1415 0:7d81654c5ce8 329 pre_bA = bA;
kambara1415 0:7d81654c5ce8 330 pre_bB = bB;
kambara1415 0:7d81654c5ce8 331 pre_bX = bX;
kambara1415 0:7d81654c5ce8 332 pre_bY = bY;
kambara1415 0:7d81654c5ce8 333 pre_bDU = bDU;
kambara1415 0:7d81654c5ce8 334 pre_bDD = bDD;
kambara1415 0:7d81654c5ce8 335 pre_bDL = bDL;
kambara1415 0:7d81654c5ce8 336 pre_bDR = bDR;
kambara1415 0:7d81654c5ce8 337 pre_x = x_point;
kambara1415 0:7d81654c5ce8 338 pre_y = y_point;
kambara1415 0:7d81654c5ce8 339
kambara1415 0:7d81654c5ce8 340 return 0;
kambara1415 0:7d81654c5ce8 341 }
kambara1415 0:7d81654c5ce8 342
kambara1415 0:7d81654c5ce8 343 int output()
kambara1415 0:7d81654c5ce8 344 {
kambara1415 0:7d81654c5ce8 345 char c;
kambara1415 0:7d81654c5ce8 346 short s;
kambara1415 0:7d81654c5ce8 347 int angle1 = ang1*442.0/PI + SERVO1_ZERO;
kambara1415 0:7d81654c5ce8 348 int angle2 = -ang2*460.0/PI + SERVO2_ZERO;
kambara1415 0:7d81654c5ce8 349 //int angle1 = SERVO1_ZERO;
kambara1415 0:7d81654c5ce8 350 //int angle2 = SERVO2_ZERO;
kambara1415 0:7d81654c5ce8 351 //int angle1 = ang1;
kambara1415 0:7d81654c5ce8 352 //int angle2 = ang2;
kambara1415 0:7d81654c5ce8 353 static int pre_angle1;
kambara1415 0:7d81654c5ce8 354 static int pre_angle2;
kambara1415 0:7d81654c5ce8 355
kambara1415 0:7d81654c5ce8 356 //double ang1a = (angle1-SERVO1_ZERO)*PI/442.0;
kambara1415 0:7d81654c5ce8 357 //double ang2a = (angle2-SERVO2_ZERO)*PI/409.0;
kambara1415 0:7d81654c5ce8 358
kambara1415 0:7d81654c5ce8 359 angle_offset = (float)(ang1 + ang2)*(float)1500.0/180.0;
kambara1415 0:7d81654c5ce8 360
kambara1415 0:7d81654c5ce8 361 int x= L1*cos(ang1) + L2*cos(ang1+ang2);
kambara1415 0:7d81654c5ce8 362 int y= L1*sin(ang1) + L2*sin(ang1+ang2);
kambara1415 0:7d81654c5ce8 363
kambara1415 0:7d81654c5ce8 364 //pc.printf("x=%d\txp=%d\ny=%d\typ=%d\n", x, x_point, y, y_point);
kambara1415 0:7d81654c5ce8 365 //pc.printf("angle=%f\n", angle);
kambara1415 0:7d81654c5ce8 366 //pc.printf("angle1=%d\tangle2=%d\n", angle1, angle2);
kambara1415 0:7d81654c5ce8 367 if(angle1 != pre_angle1){
kambara1415 0:7d81654c5ce8 368 servo1.sendShort('S', angle1);
kambara1415 0:7d81654c5ce8 369 if(servo1.readShort(&c, &s)==0){
kambara1415 0:7d81654c5ce8 370 led1=!led1;
kambara1415 0:7d81654c5ce8 371 if(c=='S'&&s==-1) angle1 = pre_angle1;
kambara1415 0:7d81654c5ce8 372 }
kambara1415 0:7d81654c5ce8 373 pre_angle1 = angle1;
kambara1415 0:7d81654c5ce8 374 }
kambara1415 0:7d81654c5ce8 375
kambara1415 0:7d81654c5ce8 376 if(angle2 != pre_angle2){
kambara1415 0:7d81654c5ce8 377 servo2.sendShort('S', angle2);
kambara1415 0:7d81654c5ce8 378 if(servo2.readShort(&c, &s)==0){
kambara1415 0:7d81654c5ce8 379 led2 = !led2;
kambara1415 0:7d81654c5ce8 380 if(c=='S'&&s==-1) angle2 = pre_angle2;
kambara1415 0:7d81654c5ce8 381 }
kambara1415 0:7d81654c5ce8 382 pre_angle2 = angle2;
kambara1415 0:7d81654c5ce8 383 }
kambara1415 0:7d81654c5ce8 384
kambara1415 0:7d81654c5ce8 385 int angle_output = RC_CENTER + angle; //+ angle_offset;
kambara1415 0:7d81654c5ce8 386
kambara1415 0:7d81654c5ce8 387 if(angle_output > RC_MAX){
kambara1415 0:7d81654c5ce8 388 angle_output = RC_MAX;
kambara1415 0:7d81654c5ce8 389 angle = RC_MAX - RC_CENTER;
kambara1415 0:7d81654c5ce8 390 }else if(angle_output < RC_MIN){
kambara1415 0:7d81654c5ce8 391 angle_output = RC_MIN;
kambara1415 0:7d81654c5ce8 392 angle = RC_MIN - RC_CENTER;
kambara1415 0:7d81654c5ce8 393 }
kambara1415 0:7d81654c5ce8 394 generatePwm_us(RC_CENTER + angle - angle_offset, 20000);
kambara1415 0:7d81654c5ce8 395
kambara1415 0:7d81654c5ce8 396 if(isVacuum == 1) pump = 1;
kambara1415 0:7d81654c5ce8 397 else pump = 0;
kambara1415 0:7d81654c5ce8 398
kambara1415 0:7d81654c5ce8 399 //pc.printf("pump=%d\n", isVacuum);
kambara1415 0:7d81654c5ce8 400
kambara1415 0:7d81654c5ce8 401 if(isRelease == 1) valve = 1;
kambara1415 0:7d81654c5ce8 402 else valve = 0;
kambara1415 0:7d81654c5ce8 403
kambara1415 0:7d81654c5ce8 404 //pc.printf("motor_p = %f\n", motor_power);
kambara1415 0:7d81654c5ce8 405
kambara1415 0:7d81654c5ce8 406 if(motor_power == 0.5) z_nout_time++;
kambara1415 0:7d81654c5ce8 407 else z_nout_time=0;
kambara1415 0:7d81654c5ce8 408
kambara1415 0:7d81654c5ce8 409 if(motor_power == 0.5 && ((z_nout_time*CONTROL_PERIOD_MS) > 300)){
kambara1415 0:7d81654c5ce8 410 stby = 0;
kambara1415 0:7d81654c5ce8 411 }else{
kambara1415 0:7d81654c5ce8 412 stby = 1;
kambara1415 0:7d81654c5ce8 413 motor = motor_power;
kambara1415 0:7d81654c5ce8 414 }
kambara1415 0:7d81654c5ce8 415 return 0;
kambara1415 0:7d81654c5ce8 416 }
kambara1415 0:7d81654c5ce8 417
kambara1415 0:7d81654c5ce8 418 //角度計算
kambara1415 0:7d81654c5ce8 419 int ang_c(double x, double y,int sign)
kambara1415 0:7d81654c5ce8 420 {
kambara1415 0:7d81654c5ce8 421 static double pre_ang1=ang1;
kambara1415 0:7d81654c5ce8 422 static double pre_ang2=ang2;
kambara1415 0:7d81654c5ce8 423 ang2 = sign*acos((x*x + y*y - L1*L1 - L2*L2)/(2.0*L1*L2));
kambara1415 0:7d81654c5ce8 424 ang1 = atan(y/x) - atan((L2*sin(ang2))/(L1+L2*cos(ang2)));
kambara1415 0:7d81654c5ce8 425 if(x<0) ang1=ang1+PI;
kambara1415 0:7d81654c5ce8 426 if(ang1 > PI) ang1 -= 2*PI;
kambara1415 0:7d81654c5ce8 427 if(ang2 > PI) ang2 -= 2*PI;
kambara1415 0:7d81654c5ce8 428
kambara1415 0:7d81654c5ce8 429 if(sqrt(x*x+y*y) > MAX_LEN || sqrt(x*x+y*y) <= MIN_LEN){
kambara1415 0:7d81654c5ce8 430 ang1 = pre_ang1;
kambara1415 0:7d81654c5ce8 431 ang2 = pre_ang2;
kambara1415 0:7d81654c5ce8 432 return -1;
kambara1415 0:7d81654c5ce8 433 }
kambara1415 0:7d81654c5ce8 434 pre_ang1 = ang1;
kambara1415 0:7d81654c5ce8 435 pre_ang2 = ang2;
kambara1415 0:7d81654c5ce8 436 return 0;
kambara1415 0:7d81654c5ce8 437 }
kambara1415 0:7d81654c5ce8 438
kambara1415 0:7d81654c5ce8 439 //SOFT PWM SERVO
kambara1415 0:7d81654c5ce8 440
kambara1415 0:7d81654c5ce8 441 #define SOFT_PWM_CYCLE 15
kambara1415 0:7d81654c5ce8 442
kambara1415 0:7d81654c5ce8 443 int soft_pwm_period=0;
kambara1415 0:7d81654c5ce8 444 int soft_pulse_width=0;
kambara1415 0:7d81654c5ce8 445
kambara1415 0:7d81654c5ce8 446 void generatePWM(){
kambara1415 0:7d81654c5ce8 447 static int count = 0;
kambara1415 0:7d81654c5ce8 448 if ((count++)*SOFT_PWM_CYCLE < soft_pulse_width)
kambara1415 0:7d81654c5ce8 449 rc_servo = 1;
kambara1415 0:7d81654c5ce8 450 else
kambara1415 0:7d81654c5ce8 451 rc_servo = 0;
kambara1415 0:7d81654c5ce8 452 if (count*SOFT_PWM_CYCLE > soft_pwm_period)
kambara1415 0:7d81654c5ce8 453 count = 0;
kambara1415 0:7d81654c5ce8 454 }
kambara1415 0:7d81654c5ce8 455
kambara1415 0:7d81654c5ce8 456 void generatePwm_us(int p_width, int p_period)
kambara1415 0:7d81654c5ce8 457 {
kambara1415 0:7d81654c5ce8 458 if(soft_pwm_period != p_period){
kambara1415 0:7d81654c5ce8 459 soft_pwm_period = p_period;
kambara1415 0:7d81654c5ce8 460 soft_pulse_width = p_width;
kambara1415 0:7d81654c5ce8 461 soft_pwm.attach_us(&generatePWM, SOFT_PWM_CYCLE);
kambara1415 0:7d81654c5ce8 462 }else soft_pulse_width = p_width;
kambara1415 0:7d81654c5ce8 463 }
kambara1415 0:7d81654c5ce8 464
kambara1415 0:7d81654c5ce8 465 void beep_A(){
kambara1415 0:7d81654c5ce8 466 speaker.tone(2000, 300);
kambara1415 0:7d81654c5ce8 467 }
kambara1415 0:7d81654c5ce8 468
kambara1415 0:7d81654c5ce8 469 void beep_B(){
kambara1415 0:7d81654c5ce8 470 speaker.tone(1000, 300);
kambara1415 0:7d81654c5ce8 471 }