ロボステ6期
/
NHK2020-arm-sub3
12/20
main.cpp
- Committer:
- aoikoizumi
- Date:
- 2019-12-20
- Revision:
- 2:90b53995cb1f
- Parent:
- 1:bc34fdc4e16b
File content as of revision 2:90b53995cb1f:
#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_motor() { while(1) { 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; pc.printf("s"); } if(turn*old_turn<0)turn=0; backdrop.turn(turn); pc.printf("%lf",EC_backdrop.getRad()); if(b-a<0.05&&a-b<0.05)break; } } 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() { pc.printf("setting please"); move_motor(); while(1) { #ifdef NHK2020_TEST_MODE printf("T2 = %d\n\r",T2); 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"); //printf("T2 = %d\n\r",T2); if(T2 == 0) { 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"); T2=1; break; } } } if(T2 == 1) { //スタート位置からボール前まで移動(アーム待機) //T2=1; while(1) { printf("a-T = 0 T2 = %d t2_r = %d\n\r",T2,t2_r); wait(0.1); if(T2 == 2) { break; } } } if(T2 == 2) { //ボール掴んで投げる //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); snatch=1; tsukami(); printf("tsukami"); move_motor(); wait(3); snatch=0; wait(1); put(); printf("put"); move_motor(); wait(3); snatch=1; wait(1); top(); printf("top"); move_motor(); wait(5); pass1=0; wait(5); //wait(5); T2++; } if(T2 == 3) { //スタートゾーンに戻る(アーム待機) while(1) { printf("a-T = 2 T2 = %d t2_r = %d\n\r",T2,t2_r); wait(0.1); if(T2 == 4) { break; } } } printf("end\n\r"); } #endif }