Ryota Kotani / Mbed 2 deprecated ROS_R2_YONRIN

ROSでR2の足回りを動かせれるようにしたやつです。 使えるのは今のところ、小谷（私）しかいませんが一応。

Dependencies:   mbed URF MD_MDDS30_oit_ ros_lib_melodic

main.cpp@0:450a7208c682, 2022-05-03 (annotated)

Committer:
akihiron
Date:
Tue May 03 08:02:58 2022 +0000
Revision:
0:450a7208c682
Child:
1:f6a9a3a7455d
05/03

Who changed what in which revision?

UserRevisionLine numberNew contents of line
akihiron 0:450a7208c682 1 #include "mbed.h"
akihiron 0:450a7208c682 2 #include "CAN.h"
akihiron 0:450a7208c682 3 #include "PS4_Controller.h"
akihiron 0:450a7208c682 4 #include "MDDS30_oit.h"
akihiron 0:450a7208c682 5 #include "URF.h"
akihiron 0:450a7208c682 6
akihiron 0:450a7208c682 7 PS4_controller PS4(PC_12,PD_2);
akihiron 0:450a7208c682 8
akihiron 0:450a7208c682 9 Serial pc(USBTX, USBRX, 115200);
akihiron 0:450a7208c682 10
akihiron 0:450a7208c682 11 URF usdRight(D6);
akihiron 0:450a7208c682 12 URF usdLeft(D8);
akihiron 0:450a7208c682 13
akihiron 0:450a7208c682 14 Ticker update;
akihiron 0:450a7208c682 15
akihiron 0:450a7208c682 16 DigitalOut led(LED1);
akihiron 0:450a7208c682 17 DigitalOut led2(PC_4);
akihiron 0:450a7208c682 18
akihiron 0:450a7208c682 19 DigitalIn btn(USER_BUTTON);
akihiron 0:450a7208c682 20 MDDS30oit MD1(PC_10, PC_11); //left:モーター番号1（右後輪）、right:モーター番号2（左後輪）
akihiron 0:450a7208c682 21 MDDS30oit MD2(PA_0, PA_1); //left:モーター番号3（右前輪）、right:モーター番号4（左前輪）
akihiron 0:450a7208c682 22
akihiron 0:450a7208c682 23 void inter(void)
akihiron 0:450a7208c682 24 {
akihiron 0:450a7208c682 25 usdRight.send();
akihiron 0:450a7208c682 26 usdLeft.send();
akihiron 0:450a7208c682 27 }
akihiron 0:450a7208c682 28
akihiron 0:450a7208c682 29 int i;
akihiron 0:450a7208c682 30 int right = 0, left = 0;
akihiron 0:450a7208c682 31 float duty[4]={0.0f};
akihiron 0:450a7208c682 32
akihiron 0:450a7208c682 33 void Yonrin(float x,float y) //四輪オムニのpwm値出し
akihiron 0:450a7208c682 34 {
akihiron 0:450a7208c682 35 duty[0] =-x/sqrtf(2)+y/sqrtf(2);
akihiron 0:450a7208c682 36 duty[1] =-x/sqrtf(2)-y/sqrtf(2);
akihiron 0:450a7208c682 37 duty[2] = x/sqrtf(2)-y/sqrtf(2);
akihiron 0:450a7208c682 38 duty[3] = x/sqrtf(2)+y/sqrtf(2);
akihiron 0:450a7208c682 39 //pc.printf("a=%f, b=%f, c=%f, d=%f\r\n",duty[0],duty[1],duty[2],duty[3]);
akihiron 0:450a7208c682 40
akihiron 0:450a7208c682 41 MD1.left(duty[0]);
akihiron 0:450a7208c682 42 MD1.right(duty[1]);
akihiron 0:450a7208c682 43 MD2.left(duty[2]);
akihiron 0:450a7208c682 44 MD2.right(duty[3]);
akihiron 0:450a7208c682 45 wait(0.01);
akihiron 0:450a7208c682 46 }
akihiron 0:450a7208c682 47
akihiron 0:450a7208c682 48 void Yonrin(float x)
akihiron 0:450a7208c682 49 {
akihiron 0:450a7208c682 50 //pc.printf("senkai = %f\r\n",x);
akihiron 0:450a7208c682 51
akihiron 0:450a7208c682 52 MD1.left(x);
akihiron 0:450a7208c682 53 MD1.right(x);
akihiron 0:450a7208c682 54 MD2.left(x);
akihiron 0:450a7208c682 55 MD2.right(x);
akihiron 0:450a7208c682 56
akihiron 0:450a7208c682 57 wait(0.01);
akihiron 0:450a7208c682 58 }
akihiron 0:450a7208c682 59
akihiron 0:450a7208c682 60 void Yonrin(int mode)
akihiron 0:450a7208c682 61 {
akihiron 0:450a7208c682 62 switch(mode) {
akihiron 0:450a7208c682 63 case 0:
akihiron 0:450a7208c682 64 MD1.left(0);
akihiron 0:450a7208c682 65 MD1.right(0);
akihiron 0:450a7208c682 66 MD2.left(0);
akihiron 0:450a7208c682 67 MD2.right(0);
akihiron 0:450a7208c682 68 }
akihiron 0:450a7208c682 69 }
akihiron 0:450a7208c682 70
akihiron 0:450a7208c682 71 int main(){
akihiron 0:450a7208c682 72
akihiron 0:450a7208c682 73 wait_ms(100);
akihiron 0:450a7208c682 74
akihiron 0:450a7208c682 75 PS4.send_UNO(1);
akihiron 0:450a7208c682 76
akihiron 0:450a7208c682 77 while(!PS4.connect()){
akihiron 0:450a7208c682 78 pc.printf("1");
akihiron 0:450a7208c682 79 led=!led;
akihiron 0:450a7208c682 80 PS4.read_PAD();
akihiron 0:450a7208c682 81 pc.printf("2");
akihiron 0:450a7208c682 82 wait_ms(500);
akihiron 0:450a7208c682 83 }
akihiron 0:450a7208c682 84 led = 1;
akihiron 0:450a7208c682 85
akihiron 0:450a7208c682 86 update.attach(&inter, 0.06);
akihiron 0:450a7208c682 87
akihiron 0:450a7208c682 88 while(1){
akihiron 0:450a7208c682 89 int dataRight = usdRight.read(mm);
akihiron 0:450a7208c682 90 int dataLeft = usdLeft.read(mm);
akihiron 0:450a7208c682 91 pc.printf("Left:%d, Right:%d\n", dataLeft, dataRight);
akihiron 0:450a7208c682 92 //pc.printf("right: %d, left: %d\n", dataRight, dataLeft);
akihiron 0:450a7208c682 93
akihiron 0:450a7208c682 94 //right = 0; //機体の右側に障害物なし
akihiron 0:450a7208c682 95 //left = 0; //機体の左側に障害物なし
akihiron 0:450a7208c682 96
akihiron 0:450a7208c682 97 PS4.read_PAD();
akihiron 0:450a7208c682 98 if(PS4.button(PS))PS4.disconnect();
akihiron 0:450a7208c682 99 if(PS4.connect()){
akihiron 0:450a7208c682 100 if(PS4.button(R1)==1){
akihiron 0:450a7208c682 101 if(PS4.analog(PS_LX)<100||PS4.analog(PS_LX)>146||PS4.analog(PS_LY)<100||PS4.analog(PS_LY)>146){
akihiron 0:450a7208c682 102 float x_duty = ((PS4.analog(PS_LX)-128.0f)/127.0f)*0.7f;
akihiron 0:450a7208c682 103 float y_duty = -((PS4.analog(PS_LY)-128.0f)/127.0f)*0.7f;
akihiron 0:450a7208c682 104 //pc.printf("x_duty=%f y_duty=%f\r\n", x_duty, y_duty);
akihiron 0:450a7208c682 105 Yonrin(x_duty, y_duty);
akihiron 0:450a7208c682 106 /*wait(1.0);
akihiron 0:450a7208c682 107 Yonrin(0);*/
akihiron 0:450a7208c682 108 }
akihiron 0:450a7208c682 109 else if(PS4.analog(PS_R2)>10){
akihiron 0:450a7208c682 110 float x=-((PS4.analog(PS_R2)/255.0f))*0.7f;
akihiron 0:450a7208c682 111 Yonrin(x);
akihiron 0:450a7208c682 112 }
akihiron 0:450a7208c682 113 else if(PS4.analog(PS_L2)>10){
akihiron 0:450a7208c682 114 float x=((PS4.analog(PS_L2)/255.0f))*0.7f;
akihiron 0:450a7208c682 115 Yonrin(x);
akihiron 0:450a7208c682 116 }
akihiron 0:450a7208c682 117 else if(PS4.button(UP)==1){
akihiron 0:450a7208c682 118 Yonrin(0,0.5f);
akihiron 0:450a7208c682 119 }
akihiron 0:450a7208c682 120 else if(PS4.button(DOWN)==1){
akihiron 0:450a7208c682 121 Yonrin(0,-0.5f);
akihiron 0:450a7208c682 122 }
akihiron 0:450a7208c682 123 else if(PS4.button(RIGHT)==1){
akihiron 0:450a7208c682 124 if (dataRight >= 400){
akihiron 0:450a7208c682 125 Yonrin(-0.5f,0);
akihiron 0:450a7208c682 126 }
akihiron 0:450a7208c682 127 else{
akihiron 0:450a7208c682 128 if (dataRight <= 400 && dataRight >= 200){
akihiron 0:450a7208c682 129 Yonrin(-0.3f, 0);
akihiron 0:450a7208c682 130 }
akihiron 0:450a7208c682 131 }
akihiron 0:450a7208c682 132 }
akihiron 0:450a7208c682 133 else if(PS4.button(LEFT)==1){
akihiron 0:450a7208c682 134 if (dataLeft >= 400){
akihiron 0:450a7208c682 135 Yonrin(0.5f,0);
akihiron 0:450a7208c682 136 }else{
akihiron 0:450a7208c682 137 if (dataLeft <= 400 && dataLeft >= 200){
akihiron 0:450a7208c682 138 Yonrin(0.3f, 0);
akihiron 0:450a7208c682 139 }
akihiron 0:450a7208c682 140 }
akihiron 0:450a7208c682 141
akihiron 0:450a7208c682 142 }
akihiron 0:450a7208c682 143 else Yonrin(0);
akihiron 0:450a7208c682 144 }
akihiron 0:450a7208c682 145 else Yonrin(0);
akihiron 0:450a7208c682 146 }
akihiron 0:450a7208c682 147 else Yonrin(0);
akihiron 0:450a7208c682 148 }
akihiron 0:450a7208c682 149 /*if(PS4.button(PS)){
akihiron 0:450a7208c682 150 PS4.disconnect();
akihiron 0:450a7208c682 151 }
akihiron 0:450a7208c682 152 Yonrin(0);*/
akihiron 0:450a7208c682 153 }