Ryota Kotani
ROSでR2の足回りを動かせれるようにしたやつです。 使えるのは今のところ、小谷（私）しかいませんが一応。
Dependencies: mbed URF MD_MDDS30_oit_ ros_lib_melodic
Diff: main.cpp
- Revision:
- 0:450a7208c682
- Child:
- 1:f6a9a3a7455d
diff -r 000000000000 -r 450a7208c682 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue May 03 08:02:58 2022 +0000
@@ -0,0 +1,153 @@
+#include "mbed.h"
+#include "CAN.h"
+#include "PS4_Controller.h"
+#include "MDDS30_oit.h"
+#include "URF.h"
+
+PS4_controller PS4(PC_12,PD_2);
+
+Serial pc(USBTX, USBRX, 115200);
+
+URF usdRight(D6);
+URF usdLeft(D8);
+
+Ticker update;
+
+DigitalOut led(LED1);
+DigitalOut led2(PC_4);
+
+DigitalIn btn(USER_BUTTON);
+MDDS30oit MD1(PC_10, PC_11); //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
+MDDS30oit MD2(PA_0, PA_1); //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）
+
+void inter(void)
+{
+ usdRight.send();
+ usdLeft.send();
+}
+
+int i;
+int right = 0, left = 0;
+float duty[4]={0.0f};
+
+void Yonrin(float x,float y) //四輪オムニのpwm値出し
+{
+ duty[0] =-x/sqrtf(2)+y/sqrtf(2);
+ duty[1] =-x/sqrtf(2)-y/sqrtf(2);
+ duty[2] = x/sqrtf(2)-y/sqrtf(2);
+ duty[3] = x/sqrtf(2)+y/sqrtf(2);
+ //pc.printf("a=%f, b=%f, c=%f, d=%f\r\n",duty[0],duty[1],duty[2],duty[3]);
+
+ MD1.left(duty[0]);
+ MD1.right(duty[1]);
+ MD2.left(duty[2]);
+ MD2.right(duty[3]);
+ wait(0.01);
+}
+
+void Yonrin(float x)
+{
+ //pc.printf("senkai = %f\r\n",x);
+
+ MD1.left(x);
+ MD1.right(x);
+ MD2.left(x);
+ MD2.right(x);
+
+ wait(0.01);
+}
+
+void Yonrin(int mode)
+{
+ switch(mode) {
+ case 0:
+ MD1.left(0);
+ MD1.right(0);
+ MD2.left(0);
+ MD2.right(0);
+ }
+}
+
+int main(){
+
+ wait_ms(100);
+
+ PS4.send_UNO(1);
+
+ while(!PS4.connect()){
+ pc.printf("1");
+ led=!led;
+ PS4.read_PAD();
+ pc.printf("2");
+ wait_ms(500);
+ }
+ led = 1;
+
+ update.attach(&inter, 0.06);
+
+ while(1){
+ int dataRight = usdRight.read(mm);
+ int dataLeft = usdLeft.read(mm);
+ pc.printf("Left:%d, Right:%d\n", dataLeft, dataRight);
+ //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
+
+ //right = 0; //機体の右側に障害物なし
+ //left = 0; //機体の左側に障害物なし
+
+ PS4.read_PAD();
+ if(PS4.button(PS))PS4.disconnect();
+ if(PS4.connect()){
+ if(PS4.button(R1)==1){
+ if(PS4.analog(PS_LX)<100||PS4.analog(PS_LX)>146||PS4.analog(PS_LY)<100||PS4.analog(PS_LY)>146){
+ float x_duty = ((PS4.analog(PS_LX)-128.0f)/127.0f)*0.7f;
+ float y_duty = -((PS4.analog(PS_LY)-128.0f)/127.0f)*0.7f;
+ //pc.printf("x_duty=%f y_duty=%f\r\n", x_duty, y_duty);
+ Yonrin(x_duty, y_duty);
+ /*wait(1.0);
+ Yonrin(0);*/
+ }
+ else if(PS4.analog(PS_R2)>10){
+ float x=-((PS4.analog(PS_R2)/255.0f))*0.7f;
+ Yonrin(x);
+ }
+ else if(PS4.analog(PS_L2)>10){
+ float x=((PS4.analog(PS_L2)/255.0f))*0.7f;
+ Yonrin(x);
+ }
+ else if(PS4.button(UP)==1){
+ Yonrin(0,0.5f);
+ }
+ else if(PS4.button(DOWN)==1){
+ Yonrin(0,-0.5f);
+ }
+ else if(PS4.button(RIGHT)==1){
+ if (dataRight >= 400){
+ Yonrin(-0.5f,0);
+ }
+ else{
+ if (dataRight <= 400 && dataRight >= 200){
+ Yonrin(-0.3f, 0);
+ }
+ }
+ }
+ else if(PS4.button(LEFT)==1){
+ if (dataLeft >= 400){
+ Yonrin(0.5f,0);
+ }else{
+ if (dataLeft <= 400 && dataLeft >= 200){
+ Yonrin(0.3f, 0);
+ }
+ }
+
+ }
+ else Yonrin(0);
+ }
+ else Yonrin(0);
+ }
+ else Yonrin(0);
+ }
+ /*if(PS4.button(PS)){
+ PS4.disconnect();
+ }
+ Yonrin(0);*/
+}
\ No newline at end of file