takahiro kambara / Mbed 2 deprecated 2016_catch_robo

koubou no program dayo.

Dependencies:   WiiClassicController_kai mbed

main.cpp@0:f8c396ee5d8e, 2016-08-23 (annotated)

Committer:
kambara1415
Date:
Tue Aug 23 06:36:03 2016 +0000
Revision:
0:f8c396ee5d8e
Child:
1:e9020a93ef86
catchrobo prog;

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 0:f8c396ee5d8e 3 #include "SerialServo.h"
kambara1415 0:f8c396ee5d8e 4
kambara1415 0:f8c396ee5d8e 5 #define CONTROL_PERIOD_MS 10
kambara1415 0:f8c396ee5d8e 6
kambara1415 0:f8c396ee5d8e 7 Serial pc(USBTX, USBRX);
kambara1415 0:f8c396ee5d8e 8 SerialServo servo1(p13,p14);
kambara1415 0:f8c396ee5d8e 9 SerialServo servo2(p28,p27);
kambara1415 0:f8c396ee5d8e 10 WiiClassicController wii(p9, p10); // SDA , SCL
kambara1415 0:f8c396ee5d8e 11
kambara1415 0:f8c396ee5d8e 12 DigitalOut rc_servo(p20);
kambara1415 0:f8c396ee5d8e 13 Ticker soft_pwm;
kambara1415 0:f8c396ee5d8e 14
kambara1415 0:f8c396ee5d8e 15 bool isStart = false; //競技開始
kambara1415 0:f8c396ee5d8e 16 bool isVacuum = false; //真空ポンプ
kambara1415 0:f8c396ee5d8e 17 bool isRelease = false; //負圧解放
kambara1415 0:f8c396ee5d8e 18 bool isZ_lower_limit = false; //z軸高さ限界
kambara1415 0:f8c396ee5d8e 19 bool isZ_upper_limit = false;
kambara1415 0:f8c396ee5d8e 20
kambara1415 0:f8c396ee5d8e 21 int x_point; //各座標
kambara1415 0:f8c396ee5d8e 22 int y_point;
kambara1415 0:f8c396ee5d8e 23 int z_point;
kambara1415 0:f8c396ee5d8e 24
kambara1415 0:f8c396ee5d8e 25 int rc_angle; //RCサーボ角度
kambara1415 0:f8c396ee5d8e 26 int motor_power; //上下モータ出力
kambara1415 0:f8c396ee5d8e 27
kambara1415 0:f8c396ee5d8e 28 int angle = 400;
kambara1415 0:f8c396ee5d8e 29 int pre_angle;
kambara1415 0:f8c396ee5d8e 30
kambara1415 0:f8c396ee5d8e 31 typedef union{
kambara1415 0:f8c396ee5d8e 32 short i;
kambara1415 0:f8c396ee5d8e 33 char b[2];
kambara1415 0:f8c396ee5d8e 34 }short_b;
kambara1415 0:f8c396ee5d8e 35
kambara1415 0:f8c396ee5d8e 36 int init();
kambara1415 0:f8c396ee5d8e 37 int check();
kambara1415 0:f8c396ee5d8e 38 int output();
kambara1415 0:f8c396ee5d8e 39 void generate_pwm_us(int, int);
kambara1415 0:f8c396ee5d8e 40
kambara1415 0:f8c396ee5d8e 41 int main() {
kambara1415 0:f8c396ee5d8e 42 init();
kambara1415 0:f8c396ee5d8e 43 while(1) {
kambara1415 0:f8c396ee5d8e 44 check();
kambara1415 0:f8c396ee5d8e 45 output();
kambara1415 0:f8c396ee5d8e 46 wait_ms(CONTROL_PERIOD_MS);
kambara1415 0:f8c396ee5d8e 47 }
kambara1415 0:f8c396ee5d8e 48 }
kambara1415 0:f8c396ee5d8e 49
kambara1415 0:f8c396ee5d8e 50 int init()
kambara1415 0:f8c396ee5d8e 51 {
kambara1415 0:f8c396ee5d8e 52 pc.baud(9600);
kambara1415 0:f8c396ee5d8e 53 servo1.start(19200, 128);
kambara1415 0:f8c396ee5d8e 54 servo2.start(28400, 128);
kambara1415 0:f8c396ee5d8e 55 return 0;
kambara1415 0:f8c396ee5d8e 56 }
kambara1415 0:f8c396ee5d8e 57
kambara1415 0:f8c396ee5d8e 58 int check()
kambara1415 0:f8c396ee5d8e 59 {
kambara1415 0:f8c396ee5d8e 60 if(wii.button_DU() == 1) angle++;
kambara1415 0:f8c396ee5d8e 61 else if(wii.button_DD() == 1) angle--;
kambara1415 0:f8c396ee5d8e 62 return 0;
kambara1415 0:f8c396ee5d8e 63 }
kambara1415 0:f8c396ee5d8e 64
kambara1415 0:f8c396ee5d8e 65 int output()
kambara1415 0:f8c396ee5d8e 66 {
kambara1415 0:f8c396ee5d8e 67 char c;
kambara1415 0:f8c396ee5d8e 68 short s;
kambara1415 0:f8c396ee5d8e 69 servo1.sendShort('S', angle);
kambara1415 0:f8c396ee5d8e 70 if(servo1.readShort(&c, &s)==0){
kambara1415 0:f8c396ee5d8e 71 if(s==1) angle = pre_angle;
kambara1415 0:f8c396ee5d8e 72 }
kambara1415 0:f8c396ee5d8e 73 pre_angle = angle;
kambara1415 0:f8c396ee5d8e 74 return 0;
kambara1415 0:f8c396ee5d8e 75 }
kambara1415 0:f8c396ee5d8e 76
kambara1415 0:f8c396ee5d8e 77
kambara1415 0:f8c396ee5d8e 78 //SOFT PWM SERVO
kambara1415 0:f8c396ee5d8e 79
kambara1415 0:f8c396ee5d8e 80 #define SOFT_PWM_CYCLE 10
kambara1415 0:f8c396ee5d8e 81
kambara1415 0:f8c396ee5d8e 82 int soft_pwm_period;
kambara1415 0:f8c396ee5d8e 83 int soft_pulse_width;
kambara1415 0:f8c396ee5d8e 84
kambara1415 0:f8c396ee5d8e 85 void generatePWM(){
kambara1415 0:f8c396ee5d8e 86 static int count = 0;
kambara1415 0:f8c396ee5d8e 87 if (count++ < soft_pulse_width/SOFT_PWM_CYCLE)
kambara1415 0:f8c396ee5d8e 88 rc_servo = 1;
kambara1415 0:f8c396ee5d8e 89 else
kambara1415 0:f8c396ee5d8e 90 rc_servo = 0;
kambara1415 0:f8c396ee5d8e 91 if (count > soft_pwm_period/SOFT_PWM_CYCLE)
kambara1415 0:f8c396ee5d8e 92 count = 0;
kambara1415 0:f8c396ee5d8e 93 }
kambara1415 0:f8c396ee5d8e 94
kambara1415 0:f8c396ee5d8e 95 void generatePwm_us(int p_width, int p_period)
kambara1415 0:f8c396ee5d8e 96 {
kambara1415 0:f8c396ee5d8e 97 soft_pwm.attach(&generatePWM, SOFT_PWM_CYCLE);
kambara1415 0:f8c396ee5d8e 98 soft_pwm_period = p_period;
kambara1415 0:f8c396ee5d8e 99 soft_pulse_width = p_width;
kambara1415 0:f8c396ee5d8e 100 }