SES / Mbed 2 deprecated RobotControl_Sample2022

767mother

Dependencies:   IncEncoder KRS mbed

main.cpp@0:e7fea65cf3ab, 22 months ago (annotated)

Committer:
koki_konishi
Date:
Tue Nov 29 07:04:04 2022 +0000
Revision:
0:e7fea65cf3ab
767mother

Who changed what in which revision?

UserRevisionLine numberNew contents of line
koki_konishi 0:e7fea65cf3ab 1 #include "mbed.h"
koki_konishi 0:e7fea65cf3ab 2 /*
koki_konishi 0:e7fea65cf3ab 3 #include "IncEncoder.h" //インクリメント方式エンコーダ用ライブラリ
koki_konishi 0:e7fea65cf3ab 4 #include "KRS.h" //近藤ICSサーボ用ライブラリ
koki_konishi 0:e7fea65cf3ab 5 */
koki_konishi 0:e7fea65cf3ab 6 #include <math.h>
koki_konishi 0:e7fea65cf3ab 7
koki_konishi 0:e7fea65cf3ab 8 //円周率
koki_konishi 0:e7fea65cf3ab 9 #define PI 3.14159265358979
koki_konishi 0:e7fea65cf3ab 10
koki_konishi 0:e7fea65cf3ab 11 //モータードライバのアドレス
koki_konishi 0:e7fea65cf3ab 12 #define DR1_MOTOR 0x01
koki_konishi 0:e7fea65cf3ab 13 #define DR2_MOTOR 0x02
koki_konishi 0:e7fea65cf3ab 14 #define DR3_MOTOR 0x03
koki_konishi 0:e7fea65cf3ab 15 #define DR4_MOTOR 0x04
koki_konishi 0:e7fea65cf3ab 16
koki_konishi 0:e7fea65cf3ab 17 //モータードライバへの命令
koki_konishi 0:e7fea65cf3ab 18 #define STOP 0
koki_konishi 0:e7fea65cf3ab 19 #define CW 1
koki_konishi 0:e7fea65cf3ab 20 #define CCW -1
koki_konishi 0:e7fea65cf3ab 21 #define bSTOP 10
koki_konishi 0:e7fea65cf3ab 22 #define bCW 11
koki_konishi 0:e7fea65cf3ab 23 #define bCCW -11
koki_konishi 0:e7fea65cf3ab 24
koki_konishi 0:e7fea65cf3ab 25 #define sCW 21
koki_konishi 0:e7fea65cf3ab 26 #define sCCW -21
koki_konishi 0:e7fea65cf3ab 27 #define sSTOP 20
koki_konishi 0:e7fea65cf3ab 28 #define shCW 31
koki_konishi 0:e7fea65cf3ab 29 #define shCCW -31
koki_konishi 0:e7fea65cf3ab 30
koki_konishi 0:e7fea65cf3ab 31 #define lCW 51
koki_konishi 0:e7fea65cf3ab 32 #define lCCW -51
koki_konishi 0:e7fea65cf3ab 33 #define lSTOP 50
koki_konishi 0:e7fea65cf3ab 34 #define lsCW 61
koki_konishi 0:e7fea65cf3ab 35 #define lsCCW -61
koki_konishi 0:e7fea65cf3ab 36 #define lsSTOP 60
koki_konishi 0:e7fea65cf3ab 37
koki_konishi 0:e7fea65cf3ab 38 #define rON 41
koki_konishi 0:e7fea65cf3ab 39 #define rOFF -41
koki_konishi 0:e7fea65cf3ab 40
koki_konishi 0:e7fea65cf3ab 41 Ticker timer; //767マザー動作不安定
koki_konishi 0:e7fea65cf3ab 42
koki_konishi 0:e7fea65cf3ab 43 Serial controller_uart(PA_0,PA_1,19200);
koki_konishi 0:e7fea65cf3ab 44 I2C md_i2c_tx(PD_13,PD_12);
koki_konishi 0:e7fea65cf3ab 45
koki_konishi 0:e7fea65cf3ab 46 DigitalOut led1(PC_10);
koki_konishi 0:e7fea65cf3ab 47 DigitalOut led2(PD_7);
koki_konishi 0:e7fea65cf3ab 48 //DigitalOut led3(PD_6);
koki_konishi 0:e7fea65cf3ab 49 //DigitalOut led4(PD_4);
koki_konishi 0:e7fea65cf3ab 50
koki_konishi 0:e7fea65cf3ab 51 volatile unsigned char controller_data[10] = { 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80 };
koki_konishi 0:e7fea65cf3ab 52 volatile unsigned char controller_data_number = 0;
koki_konishi 0:e7fea65cf3ab 53 volatile unsigned char controller_cmd_getflag = 0;
koki_konishi 0:e7fea65cf3ab 54
koki_konishi 0:e7fea65cf3ab 55 double lx, ly, lraw_rad, lclearance, ldistance, langle;
koki_konishi 0:e7fea65cf3ab 56 double rx, ry, rraw_rad, rclearance, rdistance, rangle;
koki_konishi 0:e7fea65cf3ab 57
koki_konishi 0:e7fea65cf3ab 58 //プロトタイプ関数宣言
koki_konishi 0:e7fea65cf3ab 59 void read_controller_data();
koki_konishi 0:e7fea65cf3ab 60 void con_check();
koki_konishi 0:e7fea65cf3ab 61 void joy_cal();
koki_konishi 0:e7fea65cf3ab 62 void md_setoutput(unsigned char address, int rotate, int duty);
koki_konishi 0:e7fea65cf3ab 63
koki_konishi 0:e7fea65cf3ab 64 //main
koki_konishi 0:e7fea65cf3ab 65 int main(){
koki_konishi 0:e7fea65cf3ab 66 md_i2c_tx.frequency(400000); //MD用I2C
koki_konishi 0:e7fea65cf3ab 67 timer.attach_us(&con_check, 100000);
koki_konishi 0:e7fea65cf3ab 68 controller_uart.attach(&read_controller_data, Serial::RxIrq); //受信割り込み
koki_konishi 0:e7fea65cf3ab 69
koki_konishi 0:e7fea65cf3ab 70 int rev1, rev2, rev3, rev4;
koki_konishi 0:e7fea65cf3ab 71
koki_konishi 0:e7fea65cf3ab 72 double power1, power2, power3, power4;
koki_konishi 0:e7fea65cf3ab 73 double power_up = 0;
koki_konishi 0:e7fea65cf3ab 74
koki_konishi 0:e7fea65cf3ab 75 //ジョイスティックのオフセット
koki_konishi 0:e7fea65cf3ab 76 lclearance = 60;
koki_konishi 0:e7fea65cf3ab 77 rclearance = 90;
koki_konishi 0:e7fea65cf3ab 78
koki_konishi 0:e7fea65cf3ab 79 led1 = 1;
koki_konishi 0:e7fea65cf3ab 80
koki_konishi 0:e7fea65cf3ab 81 while(true){
koki_konishi 0:e7fea65cf3ab 82 //モータ出力比計算
koki_konishi 0:e7fea65cf3ab 83 power1 = sin((langle - 45)*PI/180);
koki_konishi 0:e7fea65cf3ab 84 power2 = sin((langle - 135)*PI/180);
koki_konishi 0:e7fea65cf3ab 85 power3 = sin((langle - 225)*PI/180);
koki_konishi 0:e7fea65cf3ab 86 power4 = sin((langle - 315)*PI/180);
koki_konishi 0:e7fea65cf3ab 87
koki_konishi 0:e7fea65cf3ab 88 if(power1 >= 0){
koki_konishi 0:e7fea65cf3ab 89 rev1 = bCW;
koki_konishi 0:e7fea65cf3ab 90 }else{
koki_konishi 0:e7fea65cf3ab 91 rev1 = bCCW;
koki_konishi 0:e7fea65cf3ab 92 power1 = -power1;
koki_konishi 0:e7fea65cf3ab 93 }
koki_konishi 0:e7fea65cf3ab 94 if(power2 >= 0){
koki_konishi 0:e7fea65cf3ab 95 rev2 = bCW;
koki_konishi 0:e7fea65cf3ab 96 }else{
koki_konishi 0:e7fea65cf3ab 97 rev2 = bCCW;
koki_konishi 0:e7fea65cf3ab 98 power2 = -power2;
koki_konishi 0:e7fea65cf3ab 99 }
koki_konishi 0:e7fea65cf3ab 100 if(power3 >= 0){
koki_konishi 0:e7fea65cf3ab 101 rev3 = bCW;
koki_konishi 0:e7fea65cf3ab 102 }else{
koki_konishi 0:e7fea65cf3ab 103 rev3 = bCCW;
koki_konishi 0:e7fea65cf3ab 104 power3 = -power3;
koki_konishi 0:e7fea65cf3ab 105 }
koki_konishi 0:e7fea65cf3ab 106 if(power4 >= 0){
koki_konishi 0:e7fea65cf3ab 107 rev4 = bCW;
koki_konishi 0:e7fea65cf3ab 108 }else{
koki_konishi 0:e7fea65cf3ab 109 rev4 = bCCW;
koki_konishi 0:e7fea65cf3ab 110 power4 = -power4;
koki_konishi 0:e7fea65cf3ab 111 }
koki_konishi 0:e7fea65cf3ab 112
koki_konishi 0:e7fea65cf3ab 113 //速度チェンジ
koki_konishi 0:e7fea65cf3ab 114 if (controller_data[2] & 0x04){
koki_konishi 0:e7fea65cf3ab 115 power_up = 1;
koki_konishi 0:e7fea65cf3ab 116 }else{
koki_konishi 0:e7fea65cf3ab 117 power_up = 0.8;
koki_konishi 0:e7fea65cf3ab 118 }
koki_konishi 0:e7fea65cf3ab 119
koki_konishi 0:e7fea65cf3ab 120 //足回り処理
koki_konishi 0:e7fea65cf3ab 121 if(rx > 0){
koki_konishi 0:e7fea65cf3ab 122 md_setoutput(DR1_MOTOR, bCW, rx * 0.5);
koki_konishi 0:e7fea65cf3ab 123 md_setoutput(DR2_MOTOR, bCW, rx * 0.5);
koki_konishi 0:e7fea65cf3ab 124 md_setoutput(DR3_MOTOR, bCW, rx * 0.5);
koki_konishi 0:e7fea65cf3ab 125 md_setoutput(DR4_MOTOR, bCW, rx * 0.5);
koki_konishi 0:e7fea65cf3ab 126 }else if(rx < 0){
koki_konishi 0:e7fea65cf3ab 127 md_setoutput(DR1_MOTOR, bCCW, -rx * 0.5);
koki_konishi 0:e7fea65cf3ab 128 md_setoutput(DR2_MOTOR, bCCW, -rx * 0.5);
koki_konishi 0:e7fea65cf3ab 129 md_setoutput(DR3_MOTOR, bCCW, -rx * 0.5);
koki_konishi 0:e7fea65cf3ab 130 md_setoutput(DR4_MOTOR, bCCW, -rx * 0.5);
koki_konishi 0:e7fea65cf3ab 131 }else{
koki_konishi 0:e7fea65cf3ab 132 if(ldistance > 50){
koki_konishi 0:e7fea65cf3ab 133 md_setoutput(DR1_MOTOR, rev1, (char)power1*ldistance*power_up);
koki_konishi 0:e7fea65cf3ab 134 md_setoutput(DR2_MOTOR, rev2, (char)power2*ldistance*power_up);
koki_konishi 0:e7fea65cf3ab 135 md_setoutput(DR3_MOTOR, rev3, (char)power3*ldistance*power_up);
koki_konishi 0:e7fea65cf3ab 136 md_setoutput(DR4_MOTOR, rev4, (char)power4*ldistance*power_up);
koki_konishi 0:e7fea65cf3ab 137 }else{
koki_konishi 0:e7fea65cf3ab 138 md_setoutput(DR1_MOTOR, bSTOP, 0);
koki_konishi 0:e7fea65cf3ab 139 md_setoutput(DR2_MOTOR, bSTOP, 0);
koki_konishi 0:e7fea65cf3ab 140 md_setoutput(DR3_MOTOR, bSTOP, 0);
koki_konishi 0:e7fea65cf3ab 141 md_setoutput(DR4_MOTOR, bSTOP, 0);
koki_konishi 0:e7fea65cf3ab 142 }
koki_konishi 0:e7fea65cf3ab 143 }
koki_konishi 0:e7fea65cf3ab 144
koki_konishi 0:e7fea65cf3ab 145 wait_ms(10);
koki_konishi 0:e7fea65cf3ab 146 }
koki_konishi 0:e7fea65cf3ab 147 }
koki_konishi 0:e7fea65cf3ab 148
koki_konishi 0:e7fea65cf3ab 149 //コントローラー受信処理
koki_konishi 0:e7fea65cf3ab 150 void read_controller_data() {
koki_konishi 0:e7fea65cf3ab 151 unsigned char rxdata;
koki_konishi 0:e7fea65cf3ab 152
koki_konishi 0:e7fea65cf3ab 153 rxdata = controller_uart.getc();
koki_konishi 0:e7fea65cf3ab 154
koki_konishi 0:e7fea65cf3ab 155 if (controller_data_number == 0 && rxdata == 'S')
koki_konishi 0:e7fea65cf3ab 156 controller_data_number++;
koki_konishi 0:e7fea65cf3ab 157 else if (controller_data_number >= 1 && controller_data_number <= 7) {
koki_konishi 0:e7fea65cf3ab 158 if (controller_data_number <= 3)
koki_konishi 0:e7fea65cf3ab 159 controller_data[controller_data_number] = ~rxdata;
koki_konishi 0:e7fea65cf3ab 160 else
koki_konishi 0:e7fea65cf3ab 161 controller_data[controller_data_number] = rxdata;
koki_konishi 0:e7fea65cf3ab 162 controller_data_number++;
koki_konishi 0:e7fea65cf3ab 163 }
koki_konishi 0:e7fea65cf3ab 164 else if (controller_data_number == 8 && rxdata == 'E') {
koki_konishi 0:e7fea65cf3ab 165 controller_cmd_getflag = 0x01;
koki_konishi 0:e7fea65cf3ab 166 controller_data_number = 0;
koki_konishi 0:e7fea65cf3ab 167 led2 = 1;
koki_konishi 0:e7fea65cf3ab 168 joy_cal();
koki_konishi 0:e7fea65cf3ab 169 }
koki_konishi 0:e7fea65cf3ab 170 }
koki_konishi 0:e7fea65cf3ab 171
koki_konishi 0:e7fea65cf3ab 172 void con_check(){
koki_konishi 0:e7fea65cf3ab 173 if(controller_cmd_getflag & 0x02) {
koki_konishi 0:e7fea65cf3ab 174 controller_cmd_getflag = 0;
koki_konishi 0:e7fea65cf3ab 175
koki_konishi 0:e7fea65cf3ab 176 controller_data[1] = 0x00;
koki_konishi 0:e7fea65cf3ab 177 controller_data[2] = 0x00;
koki_konishi 0:e7fea65cf3ab 178 controller_data[3] = 0x00;
koki_konishi 0:e7fea65cf3ab 179 controller_data[4] = 0x80;
koki_konishi 0:e7fea65cf3ab 180 controller_data[5] = 0x80;
koki_konishi 0:e7fea65cf3ab 181 controller_data[6] = 0x80;
koki_konishi 0:e7fea65cf3ab 182 controller_data[7] = 0x80;
koki_konishi 0:e7fea65cf3ab 183
koki_konishi 0:e7fea65cf3ab 184 led2 = 0;
koki_konishi 0:e7fea65cf3ab 185 }else
koki_konishi 0:e7fea65cf3ab 186 controller_cmd_getflag |= 0x02;
koki_konishi 0:e7fea65cf3ab 187 }
koki_konishi 0:e7fea65cf3ab 188
koki_konishi 0:e7fea65cf3ab 189 //dualsenceジョイスティック計算
koki_konishi 0:e7fea65cf3ab 190 void joy_cal() {
koki_konishi 0:e7fea65cf3ab 191 rx = -(128 - controller_data[4]) * 2;
koki_konishi 0:e7fea65cf3ab 192 ry = (128 - controller_data[5]) * 2;
koki_konishi 0:e7fea65cf3ab 193 if (rx >= 255) rx = 255;
koki_konishi 0:e7fea65cf3ab 194 if (rx <= -255) rx = -255;
koki_konishi 0:e7fea65cf3ab 195 if (ry >= 255) ry = 255;
koki_konishi 0:e7fea65cf3ab 196 if (ry <= -255) ry = -255;
koki_konishi 0:e7fea65cf3ab 197 rraw_rad = atan2(ry, rx) / PI;
koki_konishi 0:e7fea65cf3ab 198 rdistance = hypot(rx, ry) - rclearance;
koki_konishi 0:e7fea65cf3ab 199 rdistance *= (255 / (255 - rclearance));
koki_konishi 0:e7fea65cf3ab 200 if (rdistance < 0) rdistance = 0;
koki_konishi 0:e7fea65cf3ab 201 if (rdistance >= 255) rdistance = 255;
koki_konishi 0:e7fea65cf3ab 202 rangle = (rraw_rad < 0 ? 2 + rraw_rad : rraw_rad) * 180;
koki_konishi 0:e7fea65cf3ab 203 rangle = rdistance == 0 ? 0 : rangle;
koki_konishi 0:e7fea65cf3ab 204
koki_konishi 0:e7fea65cf3ab 205 if (rx > 0) {
koki_konishi 0:e7fea65cf3ab 206 rx -= rclearance;
koki_konishi 0:e7fea65cf3ab 207 if (rx < 0) rx = 0;
koki_konishi 0:e7fea65cf3ab 208 }
koki_konishi 0:e7fea65cf3ab 209 else if (rx < 0) {
koki_konishi 0:e7fea65cf3ab 210 rx += rclearance;
koki_konishi 0:e7fea65cf3ab 211 if (rx > 0) rx = 0;
koki_konishi 0:e7fea65cf3ab 212 }
koki_konishi 0:e7fea65cf3ab 213 rx *= (255 / (255 - rclearance));
koki_konishi 0:e7fea65cf3ab 214
koki_konishi 0:e7fea65cf3ab 215 lx = -(128 - controller_data[6]) * 2;
koki_konishi 0:e7fea65cf3ab 216 ly = (128 - controller_data[7]) * 2;
koki_konishi 0:e7fea65cf3ab 217 if (lx >= 255) lx = 255;
koki_konishi 0:e7fea65cf3ab 218 if (lx <= -255) lx = -255;
koki_konishi 0:e7fea65cf3ab 219 if (ly >= 255) ly = 255;
koki_konishi 0:e7fea65cf3ab 220 if (ly <= -255) ly = -255;
koki_konishi 0:e7fea65cf3ab 221 lraw_rad = atan2(ly, lx) / PI;
koki_konishi 0:e7fea65cf3ab 222 ldistance = hypot(lx, ly) - lclearance;
koki_konishi 0:e7fea65cf3ab 223 ldistance *= (255 / (255 - lclearance));
koki_konishi 0:e7fea65cf3ab 224 if (ldistance < 0) ldistance = 0;
koki_konishi 0:e7fea65cf3ab 225 if (ldistance >= 255) ldistance = 255;
koki_konishi 0:e7fea65cf3ab 226 langle = (lraw_rad < 0 ? 2 + lraw_rad : lraw_rad) * 180;
koki_konishi 0:e7fea65cf3ab 227 langle = ldistance == 0 ? 0 : langle;
koki_konishi 0:e7fea65cf3ab 228 }
koki_konishi 0:e7fea65cf3ab 229
koki_konishi 0:e7fea65cf3ab 230 //MD用I2C通信処理
koki_konishi 0:e7fea65cf3ab 231 void md_setoutput(unsigned char address, int rotate, int duty){
koki_konishi 0:e7fea65cf3ab 232 char data[2];
koki_konishi 0:e7fea65cf3ab 233
koki_konishi 0:e7fea65cf3ab 234 // モーター駆動制御モード
koki_konishi 0:e7fea65cf3ab 235 switch (rotate) {
koki_konishi 0:e7fea65cf3ab 236 case CW: data[0] = 'F'; break;// 電圧比例制御モード（PWMがLの区間はフリー）
koki_konishi 0:e7fea65cf3ab 237 case CCW: data[0] = 'R'; break;
koki_konishi 0:e7fea65cf3ab 238 case STOP: data[0] = 'S'; break;
koki_konishi 0:e7fea65cf3ab 239
koki_konishi 0:e7fea65cf3ab 240 case bCW: data[0] = 'f'; break; // 電圧比例制御モード（PWMがLの区間はブレーキ）
koki_konishi 0:e7fea65cf3ab 241 case bCCW: data[0] = 'r'; break;
koki_konishi 0:e7fea65cf3ab 242 case bSTOP: data[0] = 's'; break;
koki_konishi 0:e7fea65cf3ab 243
koki_konishi 0:e7fea65cf3ab 244 case sCW: data[0] = 'A'; break; // 速度比例制御モード
koki_konishi 0:e7fea65cf3ab 245 case sCCW: data[0] = 'B'; break;
koki_konishi 0:e7fea65cf3ab 246 case sSTOP: data[0] = 'T'; break;
koki_konishi 0:e7fea65cf3ab 247
koki_konishi 0:e7fea65cf3ab 248 case shCW: data[0] = 'a'; break; // 速度比例制御モード（命令パケット最上位ビットに指令値9ビット目を入れることで分解能2倍）
koki_konishi 0:e7fea65cf3ab 249 case shCCW: data[0] = 'b'; break;
koki_konishi 0:e7fea65cf3ab 250
koki_konishi 0:e7fea65cf3ab 251 case lCW: data[0] = 'L'; break; // 通常LAP
koki_konishi 0:e7fea65cf3ab 252 case lCCW: data[0] = 'M'; break;
koki_konishi 0:e7fea65cf3ab 253 case lSTOP: data[0] = 'N'; break;
koki_konishi 0:e7fea65cf3ab 254 case lsCW: data[0] = 'l'; break; // 速調LAP
koki_konishi 0:e7fea65cf3ab 255 case lsCCW: data[0] = 'm'; break;
koki_konishi 0:e7fea65cf3ab 256 case lsSTOP: data[0] = 'n'; break;
koki_konishi 0:e7fea65cf3ab 257
koki_konishi 0:e7fea65cf3ab 258 case rON: data[0] = 'P'; break; //リレー駆動モード
koki_konishi 0:e7fea65cf3ab 259 case rOFF: data[0] = 'p'; break;
koki_konishi 0:e7fea65cf3ab 260 }
koki_konishi 0:e7fea65cf3ab 261
koki_konishi 0:e7fea65cf3ab 262 data[1] = duty & 0xFF;
koki_konishi 0:e7fea65cf3ab 263 md_i2c_tx.write(address << 1, data, 2); //duty送信
koki_konishi 0:e7fea65cf3ab 264 }