2018_Project-R / Mbed 2 deprecated robokonDthrow_ver3

開発途中

Dependencies:   QEI mbed

Fork of robokonDthrow_ver2 by 2018_Project-R

main.cpp@3:0bfdc0f376a6, 2018-01-15 (annotated)

Committer:
johnnyken
Date:
Mon Jan 15 08:49:44 2018 +0000
Revision:
3:0bfdc0f376a6
Parent:
2:079ba2b551a4
Child:
7:4816d59e5de0
2018/01/15; ?????; ??????pin???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5115026 0:af0259ca519f 1 //ロボコン3年投擲テスト用プログラム
johnnyken 2:079ba2b551a4 2 //一連の動作を実行できるように
johnnyken 2:079ba2b551a4 3 //チャージ -> シャトル認識 -> つかむ -> 解放(投擲) -> 再装填
e5115026 0:af0259ca519f 4
e5115026 0:af0259ca519f 5 //hファイル
e5115026 0:af0259ca519f 6 #include "mbed.h"
e5115026 0:af0259ca519f 7 #include "QEI.h"
johnnyken 2:079ba2b551a4 8 #include "ultrasonic.h"
johnnyken 2:079ba2b551a4 9 #include "VL53L0X.h"
e5115026 0:af0259ca519f 10
e5115026 0:af0259ca519f 11 //difine
e5115026 0:af0259ca519f 12
e5115026 0:af0259ca519f 13 //pin設定
johnnyken 2:079ba2b551a4 14 DigitalOut valve1(p15); //ｐ21->p15
johnnyken 2:079ba2b551a4 15 QEI rollen(p21, p22, NC, 1024); //p29->p21 p30->p22
johnnyken 2:079ba2b551a4 16
johnnyken 3:0bfdc0f376a6 17 I2C vl53l0x_i2c(p9, p10); // vl53l0xのためのI2Cクラスのインスタンス化(sda, sclの順) p28->p9,p27->p10
johnnyken 3:0bfdc0f376a6 18 Timer vl53l0x_timer; // vl53l0xのためのTimerクラスのインスタンス化
johnnyken 3:0bfdc0f376a6 19
johnnyken 3:0bfdc0f376a6 20 VL53L0X sensor(&vl53l0x_i2c, &vl53l0x_timer); // i2cとtimerの実態のポインタを渡す
johnnyken 3:0bfdc0f376a6 21
johnnyken 3:0bfdc0f376a6 22 PwmOut servo(p23);//サーボのやつ
e5115026 0:af0259ca519f 23
e5115026 0:af0259ca519f 24 DigitalOut led1(LED1);
e5115026 0:af0259ca519f 25 DigitalOut led2(LED2);
e5115026 0:af0259ca519f 26 DigitalOut led3(LED3);
e5115026 0:af0259ca519f 27 DigitalOut led4(LED4);
e5115026 0:af0259ca519f 28
johnnyken 3:0bfdc0f376a6 29 InterruptIn button_hand(p5);
johnnyken 3:0bfdc0f376a6 30 InterruptIn button_zero(p6);
johnnyken 3:0bfdc0f376a6 31 InterruptIn button_charge(p7);
johnnyken 2:079ba2b551a4 32
e5115026 0:af0259ca519f 33 Serial pc(USBTX,USBRX);
e5115026 0:af0259ca519f 34
e5115026 0:af0259ca519f 35
johnnyken 2:079ba2b551a4 36
e5115026 0:af0259ca519f 37 //timer初期化
e5115026 0:af0259ca519f 38 Ticker rollening;
johnnyken 3:0bfdc0f376a6 39 Ticker tick;//
e5115026 0:af0259ca519f 40
e5115026 0:af0259ca519f 41 //グローバル変数
johnnyken 2:079ba2b551a4 42 int enc_hand = 0;
johnnyken 2:079ba2b551a4 43 //int enc_belt = 0;
johnnyken 2:079ba2b551a4 44 char state = '0';
johnnyken 3:0bfdc0f376a6 45 int distance = 0;
johnnyken 1:e7357f0f664b 46 int enc_old = 0; //過去カウント比較用
johnnyken 3:0bfdc0f376a6 47 double ServoDeg = 0;
e5115026 0:af0259ca519f 48
e5115026 0:af0259ca519f 49 //グローバル関数
johnnyken 3:0bfdc0f376a6 50
johnnyken 3:0bfdc0f376a6 51 //ハンドエンコーダ割り込み
e5115026 0:af0259ca519f 52 void Dthrow(){
johnnyken 2:079ba2b551a4 53 enc_hand = rollen.getPulses();
johnnyken 2:079ba2b551a4 54
johnnyken 2:079ba2b551a4 55 //pc.printf("%d\n",enc_hand);
johnnyken 2:079ba2b551a4 56 switch (state){
johnnyken 2:079ba2b551a4 57 case '0': // Rotate only Motor1 by Sabertooth responding to aIn_1 input.
johnnyken 2:079ba2b551a4 58 if(enc_hand < -524){ //角度入力 deg(90)
johnnyken 2:079ba2b551a4 59 led1 = 1;
johnnyken 2:079ba2b551a4 60 valve1 = 1;
johnnyken 2:079ba2b551a4 61 }
johnnyken 2:079ba2b551a4 62 else{
johnnyken 2:079ba2b551a4 63 led1 = 0;
johnnyken 2:079ba2b551a4 64 led2 = 0;
johnnyken 2:079ba2b551a4 65 led3 = 0;
johnnyken 2:079ba2b551a4 66 led4 = 0;
johnnyken 2:079ba2b551a4 67 valve1 = 0;
johnnyken 2:079ba2b551a4 68 }
johnnyken 2:079ba2b551a4 69
johnnyken 2:079ba2b551a4 70 break;
johnnyken 2:079ba2b551a4 71 case '1': // Rotate only Motor2 by Sabertooth responding to aIn_2 input.
johnnyken 2:079ba2b551a4 72 if(enc_hand < -455){ //角度入力 deg(80)
johnnyken 2:079ba2b551a4 73 led2 = 1;
johnnyken 2:079ba2b551a4 74 valve1 = 1;
johnnyken 2:079ba2b551a4 75 }
johnnyken 2:079ba2b551a4 76 else{
johnnyken 2:079ba2b551a4 77 led1 = 0;
johnnyken 2:079ba2b551a4 78 led2 = 0;
johnnyken 2:079ba2b551a4 79 led3 = 0;
johnnyken 2:079ba2b551a4 80 led4 = 0;
johnnyken 2:079ba2b551a4 81 valve1 = 0;
johnnyken 2:079ba2b551a4 82 }
johnnyken 2:079ba2b551a4 83 break;
johnnyken 2:079ba2b551a4 84 case '2': // Stop both Motor1 and Moter2 by Sabertooth.
johnnyken 2:079ba2b551a4 85 if(enc_hand < -427){ //角度入力 deg(75)
johnnyken 2:079ba2b551a4 86 led3 = 1;
johnnyken 2:079ba2b551a4 87 valve1 = 1;
johnnyken 2:079ba2b551a4 88 }
johnnyken 2:079ba2b551a4 89 else{
johnnyken 2:079ba2b551a4 90 led1 = 0;
johnnyken 2:079ba2b551a4 91 led2 = 0;
johnnyken 2:079ba2b551a4 92 led3 = 0;
johnnyken 2:079ba2b551a4 93 led4 = 0;
johnnyken 2:079ba2b551a4 94 valve1 = 0;
johnnyken 2:079ba2b551a4 95 }
johnnyken 2:079ba2b551a4 96 break;
johnnyken 2:079ba2b551a4 97 /*case '3': // Rotate only Motor1 by Sabertooth responding to aIn_1 input.
johnnyken 2:079ba2b551a4 98 if(enc_hand < - /360*1024){ //角度入力
johnnyken 2:079ba2b551a4 99 led2 = 1;
johnnyken 2:079ba2b551a4 100 valve1 = 1;
johnnyken 2:079ba2b551a4 101 }
johnnyken 2:079ba2b551a4 102 else{
johnnyken 2:079ba2b551a4 103 led2 = 0;
johnnyken 2:079ba2b551a4 104 valve1 = 0;
johnnyken 2:079ba2b551a4 105 }
johnnyken 2:079ba2b551a4 106 break;
johnnyken 2:079ba2b551a4 107 case '4': // Rotate only Motor2 by Sabertooth responding to aIn_2 input.
johnnyken 2:079ba2b551a4 108 if(enc_hand < - /360*1024){ //角度入力
johnnyken 2:079ba2b551a4 109 led2 = 1;
johnnyken 2:079ba2b551a4 110 valve1 = 1;
johnnyken 2:079ba2b551a4 111 }
johnnyken 2:079ba2b551a4 112 else{
johnnyken 2:079ba2b551a4 113 led2 = 0;
johnnyken 2:079ba2b551a4 114 valve1 = 0;
johnnyken 2:079ba2b551a4 115 }
johnnyken 2:079ba2b551a4 116 break;
johnnyken 2:079ba2b551a4 117 case '5': // Stop both Motor1 and Moter2 by Sabertooth.
johnnyken 2:079ba2b551a4 118 if(enc_hand < - /360*1024){ //角度入力
johnnyken 2:079ba2b551a4 119 led2 = 1;
johnnyken 2:079ba2b551a4 120 valve1 = 1;
johnnyken 2:079ba2b551a4 121 }
johnnyken 2:079ba2b551a4 122 else{
johnnyken 2:079ba2b551a4 123 led2 = 0;
johnnyken 2:079ba2b551a4 124 valve1 = 0;
johnnyken 2:079ba2b551a4 125 }
johnnyken 2:079ba2b551a4 126 break;*/
e5115026 0:af0259ca519f 127 }
johnnyken 2:079ba2b551a4 128 /*if(enc_hand<enc_old){
johnnyken 1:e7357f0f664b 129 led1 = 1;
johnnyken 1:e7357f0f664b 130 led3 = 0;
johnnyken 1:e7357f0f664b 131 }
johnnyken 1:e7357f0f664b 132 else{
johnnyken 1:e7357f0f664b 133 led1 = 0;
johnnyken 1:e7357f0f664b 134 led3 = 1;
johnnyken 2:079ba2b551a4 135 }*/
johnnyken 2:079ba2b551a4 136
johnnyken 2:079ba2b551a4 137 enc_old = enc_hand;
johnnyken 2:079ba2b551a4 138 }
johnnyken 2:079ba2b551a4 139
johnnyken 3:0bfdc0f376a6 140 //超音波距離センサ測定
johnnyken 2:079ba2b551a4 141 void dist(int distance){
johnnyken 2:079ba2b551a4 142 //put code here to execute when the distance has changed
johnnyken 3:0bfdc0f376a6 143 //printf("Distance_1 %d mm\r\n", distance);
johnnyken 2:079ba2b551a4 144 //printf("Distance_2 %d mm\r\n", distance);
johnnyken 2:079ba2b551a4 145 }
johnnyken 2:079ba2b551a4 146
johnnyken 3:0bfdc0f376a6 147 ultrasonic mu(p25, p26, .1, 1, &dist); //trig=>p6->p25,echo=>p7->p26
johnnyken 2:079ba2b551a4 148
johnnyken 3:0bfdc0f376a6 149 //スイッチ＠ハンド
johnnyken 2:079ba2b551a4 150 void hand(){
johnnyken 3:0bfdc0f376a6 151 valve1=0
johnnyken 3:0bfdc0f376a6 152 ServoDeg = 180;
johnnyken 3:0bfdc0f376a6 153 servo.pulsewidth(ServoDeg/100000+0.0006);
johnnyken 3:0bfdc0f376a6 154 wait(0.25);
e5115026 0:af0259ca519f 155 }
johnnyken 3:0bfdc0f376a6 156
johnnyken 3:0bfdc0f376a6 157 //スイッチ＠ゼロ点
johnnyken 2:079ba2b551a4 158 void zero(){
johnnyken 2:079ba2b551a4 159 enc_hand = 0;
johnnyken 2:079ba2b551a4 160 }
johnnyken 3:0bfdc0f376a6 161
johnnyken 3:0bfdc0f376a6 162 //スイッチ＠タイミングベルト
johnnyken 2:079ba2b551a4 163 void charge(){
johnnyken 2:079ba2b551a4 164
johnnyken 2:079ba2b551a4 165 }
johnnyken 3:0bfdc0f376a6 166
johnnyken 3:0bfdc0f376a6 167 //ToF割り込み
johnnyken 3:0bfdc0f376a6 168 void warikomi(){
johnnyken 2:079ba2b551a4 169
johnnyken 3:0bfdc0f376a6 170 }
johnnyken 3:0bfdc0f376a6 171
e5115026 0:af0259ca519f 172 //main関数
e5115026 0:af0259ca519f 173 int main(){
johnnyken 3:0bfdc0f376a6 174 pc.baud(9600);
johnnyken 3:0bfdc0f376a6 175
johnnyken 3:0bfdc0f376a6 176
johnnyken 3:0bfdc0f376a6 177
johnnyken 3:0bfdc0f376a6 178 sensor.init(); // vl53l0xの初期化
johnnyken 3:0bfdc0f376a6 179 sensor.setTimeout(500); // vl53l0xのタイムアウト時間の設定
johnnyken 3:0bfdc0f376a6 180 sensor.startContinuous(); // vl53l0xの連続データ取得開始
johnnyken 3:0bfdc0f376a6 181
johnnyken 2:079ba2b551a4 182 button_hand.mode(PullUp);
johnnyken 2:079ba2b551a4 183 button_zero.mode(PullUp);
johnnyken 2:079ba2b551a4 184 button_charge.mode(PullUp);
johnnyken 3:0bfdc0f376a6 185 //button_hand.rise(&hand);
johnnyken 3:0bfdc0f376a6 186 //button_zero.rise(&zero);
johnnyken 3:0bfdc0f376a6 187 //button_charge.rise(&charge);
johnnyken 3:0bfdc0f376a6 188
johnnyken 2:079ba2b551a4 189 mu.startUpdates();//start measuring the distance
johnnyken 3:0bfdc0f376a6 190
e5115026 0:af0259ca519f 191 rollening.attach(&Dthrow, 0.01);
johnnyken 3:0bfdc0f376a6 192 //tick.attach(&warikomi, 0.01); // setup ticker to call flip every 0.01 seconds
johnnyken 3:0bfdc0f376a6 193
johnnyken 3:0bfdc0f376a6 194 valve1 = 1;//ハンドの初期状態は開
johnnyken 3:0bfdc0f376a6 195
johnnyken 3:0bfdc0f376a6 196 servo.period(0.020);
e5115026 0:af0259ca519f 197
e5115026 0:af0259ca519f 198 while(1){
johnnyken 2:079ba2b551a4 199 mu.checkDistance();
johnnyken 3:0bfdc0f376a6 200 pc.printf("state:%c\tencoder:%d\tdistance:%d\n",state,enc_hand,distance);
johnnyken 2:079ba2b551a4 201 if(pc.readable()) state = pc.getc();
johnnyken 3:0bfdc0f376a6 202
johnnyken 2:079ba2b551a4 203 }
e5115026 0:af0259ca519f 204
johnnyken 2:079ba2b551a4 205 }