ロボステ6期
/
NHK2020-arm-sub2
12/18
Diff: main.cpp
- Revision:
- 0:6e2abd0956f1
- Child:
- 1:bc34fdc4e16b
diff -r 000000000000 -r 6e2abd0956f1 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Dec 14 12:42:13 2019 +0000 @@ -0,0 +1,250 @@ +#include "mbed.h" +#include "EC.h" //Encoderライブラリをインクルード +#include "SpeedController.h" +#define RESOLUTION 2048//分解能 + +//#define NHK2020_TEST_MODE +#define NHK2020_MAIN_MODE + +CAN can1(p30,p29); + +Ticker can_ticker; //can用ticker + +Ec4multi EC_backdrop(p15,p16,RESOLUTION); +SpeedControl backdrop(p21,p22,50,EC_backdrop); +Serial pc(USBTX,USBRX); +DigitalOut snatch(p8); +DigitalOut pass1(p27); +DigitalOut pass2(p28); +//Ticker motor_tick; //角速度計算用ticker + +char t2[1]= {0}; //動作番号(送信する値(char型)) +int t2_r=0, T2=0; //動作番号(受け取った値、送信する値(int型)) + +double a=0;//now +double b=0;//target +double turn=0; +int k = 0; +void tsukami() +{ + b=1.3; +} +void put() +{ + b=-0.7; +} +void top() +{ + b=0; +} +void move() +{ + double old_turn=turn; + a=EC_backdrop.getRad(); + if(a-b>=0.1) { + turn=0.1; + //pc.printf("F"); + + } else if (a-b>=0.05) { + turn=10*(a-b)*(a-b); + //pc.printf("f"); + } else if (b-a>=0.1) { + turn=-0.1; + //pc.printf("B"); + } else if (b-a>=0.05) { + turn=-10*(a-b)*(a-b); + //pc.printf("b"); + } else { + backdrop.stop(); + backdrop.turn(0); + turn=0; + //break; + //pc.printf("s"); + } + if(turn*old_turn<0)turn=0; + backdrop.turn(turn); +} + +int q = 0; + +void can_read() +{ + + + CANMessage msg; + + if(can1.read(msg)) { + + if(msg.id == 1) { + t2_r = msg.data[0]; + //printf("arm T2 = %d t2_r = %d\n\r",T2,t2_r); + } else { + if(q > 100) { + //printf("id1 fale\n\r"); + q = 0; + } + q++; + } + + } else { + if(k > 100) { + //printf("arm fale\n\r"); + k = 0; + } + k++; + } + + t2[0] = T2; + + if(can1.write(CANMessage(2,t2,1))) { + } + + if(t2_r > T2) { + T2 = t2_r; + } + + +} + + +int main() +{ + move(); + + pc.printf("setting please"); + while(1) { + + + + +#ifdef NHK2020_TEST_MODE + + pc.printf("%lf",EC_backdrop.getRad()); + if(pc.readable()) { + char sel=pc.getc(); + if(sel=='z') { + pc.printf("z\r\n"); + tsukami(); + } else if(sel=='x') { + pc.printf("x\r\n"); + put(); + } else if(sel=='c') { + pc.printf("c\r\n"); + top(); + } + /* if(sel=='q') { + printf("\r\n"); + if(denjiben==0)denjiben=1; + else denjiben=0; + }*/ + if(sel=='1') { + snatch=0; + printf("snatch_off\r\n"); + } + if(sel=='2') { + snatch=1; + printf("snatch_on\r\n"); + } + if(sel=='3') { + pass1=0; + printf("pass1_off\r\n"); + } + if(sel=='4') { + pass1=1; + printf("pass1_on\r\n"); + } + if(sel=='5') { + pass2=0; + printf("pass2_off\r\n"); + } + if(sel=='6') { + pass2=1; + printf("pass2_on\r\n"); + } + // if(sel=='a') { + // //pc.printf("x\r\n"); + // //put(); + // } + } + } +#endif +#ifdef NHK2020_MAIN_MODE + can1.frequency(1000000); + can_ticker.attach(&can_read,0.01); + printf("wait_mode\r\n"); + while(1) { + char sel=pc.getc(); + if(sel=='1') { + snatch=0; + printf("snatch_off\r\n"); + } + if(sel=='2') { + snatch=1; + printf("snatch_on\r\n"); + } + if(sel=='3') { + pass1=0; + printf("pass1_off\r\n"); + } + if(sel=='4') { + pass1=1; + printf("pass1_on\r\n"); + } + if(sel=='5') { + pass2=0; + printf("pass2_off\r\n"); + } + if(sel=='6') { + pass2=1; + printf("pass2_on\r\n"); + } + if(sel=='q') { + printf("end_wait_mode\r\n"); + break; + + } + } + + if(T2 == 0) { //スタート位置からボール前まで移動(アーム待機) + T2=1; + while(1) { + //printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); + wait(0.1); + if(T2 == 1) { + break; + } + } + } + + if(T2 == 1) { //ボール掴んで投げる + //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); + tsukami(); + move(); + wait(3); + snatch=0; + wait(1); + put(); + wait(3); + snatch=1; + wait(1); + top(); + wait(5); + pass1=0; + wait(5); + //wait(5); + + T2++; + } + if(T2 == 2) { //スタートゾーンに戻る(アーム待機) + while(1) { + //printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); + wait(0.1); + if(T2 == 1) { + break; + } + } + } + printf("end\n\r"); +} +#endif +} \ No newline at end of file