![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
another
Dependencies: mbed KondoServoLibrary Encoder
User.cpp
- Committer:
- o2132613
- Date:
- 2020-03-13
- Revision:
- 23:0c6bc29c8f86
- Parent:
- 22:f0e17f3129cc
- Child:
- 24:674b011da45b
File content as of revision 23:0c6bc29c8f86:
#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,ball_count=0,box_count=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);//サーボ 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);//エアシリンダー(投擲) DigitalOut out3(p18);//エアシリンダー(ボックス) int id = 0; int id2 = 1; double SERVO2DEG = 270.0 / (11500 - 3500); double first_ball = (6800 - 3500) * SERVO2DEG; double first_box = (X - 3500) * SERVO2DEG; //X haven't be decided double grab_ball = (3600 - 3500) * SERVO2DEG; double pass = (5000 - 3500) * SERVO2DEG; double grab_box = (3550 - 3500) * SERVO2DEG; void cal() { EC.calOmega(); } void shot(); void catch_ball(); void show_angle(); 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 ball = 1; led2 = 1; } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△ box = 1; led3 = 1; } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□ ball = 2; led4 = 1; } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2 box = 2; led4 = 1; } show_angle(); //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 show_angle() { 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); if(X_count ==1) { printf("set ok"); EC.reset(); } //角度リセット a=EC.getOmega(); count=EC.getCount(); i =count%500; printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count); f=0; b=0; led1=0; led2=1; led3=1; led4=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); while(1) { printf("set"); if(X_count ==1) { EC.reset(); X_count =0; } break; } //角度リセット 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 (ball) { case 1: //ball servo if(ball_count ==0) { servo.set_degree(id, first_ball); wait(0.5); ball_count =1; ball = 0; } else if(ball_count ==1) { servo.set_degree(id, grab_ball); wait(0.5); ball_count = 0; ball =0; } break; case 2://ball air if (out1==0) { out1=1; ball=0; } else if (out1 == 1) { out1=0; ball=0; } } } void catch_box() { switch(box) { case 1:// box servo if(box_count ==0) { servo.set_degree(id2, first_box); wait(0.5); box_count =1; box = 1; } else if(box_count ==1) { servo.set_degree(id2, grab_box); wait(0.5); box_count = 0; box =0; } case 2: //box air if (out3==0) { out3=1; box = 0; } else if (out3 == 1) { out3=0; box = 0; } } }