kohei matsumoto
test
Dependencies: mbed KondoServoLibrary Encoder
Diff: User.cpp
- Revision:
- 19:510b87211851
- Parent:
- 18:f7c587f0f8b8
- Child:
- 20:dd4634500bb3
--- a/User.cpp Fri Nov 01 01:39:57 2019 +0000
+++ b/User.cpp Wed Mar 11 09:08:05 2020 +0000
@@ -3,29 +3,56 @@
#include "hci.h"
#include "ps3.h"
#include "User.h"
-#define speed 0.2
+#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 air(p24);
-PwmOut motorRB(p22);
-PwmOut motorRF(p21);
-PwmOut motorLB(p25);
-PwmOut motorLF(p26);
-SPI spi(p5,p6,p7);
-DigitalOut cs(p8);
-int send;
+//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()
{
- spi.format(8,3);
- spi.frequency(1000000);
- motorRF.period_ms(1);
- motorRB.period_ms(1);
- motorLF.period_ms(1);
- motorLB.period_ms(1);
}
void UserLoop(char n,const u8* data)
{
@@ -50,68 +77,136 @@
ButtonState = ((ps3report*)(data + 1))->ButtonState;
}
//ここより下にプログラムを書く
- //spi通信用プログラム
- send = 0;
- if((ButtonState >> BUTTONUP)&1 == 1)
- {//対応するボタンを書く(今回上ボタン
- motorRF = 0.2;
- motorRB = 0;
- motorLF = 0.2;
- motorLB = 0;
- send = 0;
- }else if((ButtonState >> BUTTONDOWN)&1 == 1)
- {//対応するボタンを書く(今回下ボタン
- motorRF = 0;
- motorRB = 0.2;
- motorLF = 0;
- motorLB = 0.2;
- send = 0;
- }else if((ButtonState >> BUTTONL1)&1 == 1)
- {//対応するボタンを書く(今回L1ボタン
- motorRF = 0.2;
- motorRB = 0;
- motorLF = 0;
- motorLB = 0.2;
- send = 0;
- }else if((ButtonState >> BUTTONR1)&1 == 1)
- {//対応するボタンを書く(今回R1ボタン
- send = 0;
- motorRF = 0;
- motorRB = 0.2;
- motorLF = 0.2;
- motorLB = 0;
- }else if((ButtonState >> BUTTONRIGHT)&1 == 1)//Right
- {
- send = 4;
- motorRF = 0;
- motorRB = 0.2;
- motorLF = 0.2;
- motorLB = 0;
- }else if((ButtonState >> BUTTONLEFT)&1 == 1)//Left
- {
- send = 3;
- motorRF = 0.2;
- motorRB = 0;
- motorLF = 0;
- motorLB = 0.2;
- }else{
- send = 0;
- motorRF = 0;
- motorRB = 0;
- motorLF = 0;
- motorLB = 0;
+ 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;
}
- if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//対応するボタンを書く(今回Oボタン
- air=1;
- } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//対応するボタンを書く(今回×ボタン
- air=0;
+ //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;
}
- if((ButtonState >> BUTTONR2&1) == 1 )//R2 down
- {
- send = 1;
- }else if((ButtonState >> BUTTONL2)&1 == 1)//L2 up
- {
- send = 2;
+ //角度リセット
+ 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;
+ }
+ }
}
- spi. write(send);
+ 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()
+{
+
}
\ No newline at end of file