takahiro kambara / Mbed 2 deprecated 2016_catch_robo

koubou no program dayo.

Dependencies:   WiiClassicController_kai mbed

main.cpp@3:14f0a05e4166, 2016-09-08 (annotated)

Committer:
kambara1415
Date:
Thu Sep 08 12:04:07 2016 +0000
Revision:
3:14f0a05e4166
Parent:
2:b7a0c457aea2 
???????????

Who changed what in which revision?

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