ロボステ6期
/
ball_snatch
e
Diff: main.cpp
- Revision:
- 0:a40701dd7f26
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Dec 13 06:49:35 2019 +0000 @@ -0,0 +1,104 @@ +#include "mbed.h" +#include "EC.h" //Encoderライブラリをインクルード +#include "SpeedController.h" +#define RESOLUTION 2048//分解能 + +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 +double a=0;//now +double b=0;//target +double turn=0; +void tsukami() +{ + b=1.3; +} +void put() +{ + b=-0.7; +} +void top() +{ + b=0; +} +int main() +{ + pc.printf("setting please"); + 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(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(); + // } + } + } +} \ No newline at end of file