test
Dependencies: mbed KondoServoLibrary Encoder
User.cpp
- Committer:
- o2132613
- Date:
- 2020-03-11
- Revision:
- 19:510b87211851
- Parent:
- 18:f7c587f0f8b8
- Child:
- 20:dd4634500bb3
File content as of revision 19:510b87211851:
#include "Utils.h" #include "USBHost.h" #include "hci.h" #include "ps3.h" #include "User.h" #include "mbed.h" #include "math.h" #include "mbed.h" #include"KondoServo.h" #include "EC.h" //Encoderライブラリをインクルード #define RESOLUTION 500 int RSX,RSY,LSX,LSY,BSU,BSL; //Digitalout cs(p8); int move=5,shoot=0,ball=0,box=0; CAN controller(p30,p29);//CANpin_name DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); char can[1]= {0}; KondoServo servo(p28,p27);//サーボ int out = 1; // shot PwmOut f(p26);//投F PwmOut b(p25);//投B Ticker ticker; Ec1multi EC(p16,p17,RESOLUTION);//投E InterruptIn X(p15); DigitalOut out1(p19);//エアシリンダー(つかむ方) DigitalOut out2(p20);//エアシリンダー(投擲) int id = 0; double SERVO2DEG = 270.0 / (11500 - 3500); double first = (6800 - 3500) * SERVO2DEG; double grab = (3800 - 3500) * SERVO2DEG; double pass = (5000 - 3500) * SERVO2DEG; void cal() { EC.calOmega(); } void shot(); void catch_ball(); int X_count=0; void Xcount() { X_count++; } void UserLoopSetting() { } void UserLoop(char n,const u8* data) { u16 ButtonState; if(n==0) { //有線Ps3USB.cpp RSX = ((ps3report*)data)->RightStickX; RSY = ((ps3report*)data)->RightStickY; LSX = ((ps3report*)data)->LeftStickX; LSY = ((ps3report*)data)->LeftStickY; BSU = (u8)(((ps3report*)data)->ButtonState & 0x00ff); BSL = (u8)(((ps3report*)data)->ButtonState >> 8); //ボタンの処理 ButtonState = ((ps3report*)data)->ButtonState; } else {//無線TestShell.cpp RSX = ((ps3report*)(data + 1))->RightStickX; RSY = ((ps3report*)(data + 1))->RightStickY; LSX = ((ps3report*)(data + 1))->LeftStickX; LSY = ((ps3report*)(data + 1))->LeftStickY; BSU = (u8)(((ps3report*)(data + 1))->ButtonState & 0x00ff); BSL = (u8)(((ps3report*)(data + 1))->ButtonState >> 8); //ボタンの処理 ButtonState = ((ps3report*)(data + 1))->ButtonState; } //ここより下にプログラムを書く controller.frequency(1000000); led1=0; led2=0; led3=0; led4=0; //move if((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //UPL1 move = 9; } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //DownR1 move = 3; } else if ((ButtonState >> BUTTONUP)&1 == 1&&(ButtonState >> BUTTONR1)&1 == 1) { //UPR1 move = 7; } else if ((ButtonState >> BUTTONDOWN)&1 == 1&&(ButtonState >> BUTTONL1)&1 == 1) { //DownL1 move = 1; } else if((ButtonState >> BUTTONUP)&1 == 1) { //up move = 8; led1 = 1; } else if((ButtonState >> BUTTONDOWN)&1 == 1) { //down move = 2; led2 =1; } else if((ButtonState >> BUTTONRIGHT)&1 == 1) { //right move = 6; led3 = 1; } else if((ButtonState >> BUTTONLEFT)&1 == 1) { //left move = 4; led4 = 1; } else if((ButtonState >> BUTTONL1)&1 == 1) { //L1 move = 10; } else if((ButtonState >> BUTTONR1)&1 == 1) { //R1 move = 11; } else if((ButtonState >> BUTTONSELECT)&1 == 1 ) {//対応するボタンを書く(今回SELLECTボタン move = 1; } else { move = 5; } //move //shoot if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O shoot = 1; led1 = 1; } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X shoot = 0; led2 = 1; } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△ shoot = 3; led3 = 1; } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□ shoot = 4; led4 = 1; } //CAN通信用プログラム if(shoot==1)shot(); if(shoot>=2)catch_ball(); can[0] = move; controller.write(CANMessage(1,can,1)); printf("%d\r\n",can[0]); } void shot() { X.rise(&Xcount); f.period_us(50); b.period_us(50); double a=0,r=0.4,v=0; int i=0,count;//rで半径指定 a*r=v ticker.attach(&cal,0.05); printf("set"); if(X_count ==1) { EC.reset(); X_count =0; } //角度リセット while(1) { printf("%.3f %.3f %d %d\r\n",a,v,X_count,i); a=EC.getOmega(); count=EC.getCount(); i =count%500; v=a*r; f=0.45;//速度一定 b=0.0; led1=1; led2=0; led3=0; led4=1; if(X_count>3) { if(i < -380 && i > -390) {//離す角度を決める out2=1; //ボールを離す printf("shot\r\n"); led1=1; led2=1; led3=1; led4=1; break; } } } printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,i); X.rise(&Xcount); a=EC.getOmega(); count=EC.getCount(); i =count%500; f=0; b=0; led1=0; led2=1; led3=1; led4=0; } void catch_ball() { switch (shoot) { case 2: servo.set_degree(id, pass); break; case 3: servo.set_degree(id, grab); break; case 4: if (out1==0){ out1=1; }else if (out1 == 1){ out1=0; } } } void catch_box() { }