Kanta Takasu / Mbed 2 deprecated YUKA_tks

yuka_tks

Dependencies:   mbed HCSR04

main.cpp@6:dd35cc3b5ca0, 2021-12-20 (annotated)

Committer:
tks1
Date:
Mon Dec 20 15:49:55 2021 +0000
Revision:
6:dd35cc3b5ca0
Parent:
5:f21e1a75f98b 
yuka_tks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tomo1213 4:1bd08c9d92a9 1 //2021/12/20更新
tks1 6:dd35cc3b5ca0 2 //aoki
tks1 6:dd35cc3b5ca0 3 #include "yuka.h"
tks1 6:dd35cc3b5ca0 4 Timer t_obj;
tks1 6:dd35cc3b5ca0 5 double auto_time = 0;
tks1 6:dd35cc3b5ca0 6 int main()
tks1 6:dd35cc3b5ca0 7 {
ngokystk 0:b4b94eb28093 8 pc.attach(SerialRX);
ngokystk 0:b4b94eb28093 9 //pc.baud(115200);
ngokystk 0:b4b94eb28093 10 //CAN
ngokystk 0:b4b94eb28093 11 canPort.frequency(1000000); //Bit Rate:1MHz
ngokystk 0:b4b94eb28093 12 canPort.attach(CANdataRX,CAN::RxIrq);
tks1 6:dd35cc3b5ca0 13 //CAN node Setting
tks1 6:dd35cc3b5ca0 14 int node1 = 1;
ngokystk 0:b4b94eb28093 15 int node2 = 2;
ngokystk 0:b4b94eb28093 16 int node3 = 3;
ngokystk 0:b4b94eb28093 17 int node4 = 4;
tks1 6:dd35cc3b5ca0 18
ngokystk 0:b4b94eb28093 19 //エンコーダ関係
tks1 6:dd35cc3b5ca0 20 int ActPos1 = 0;
tks1 6:dd35cc3b5ca0 21 int ActPos2 = 0;
ngokystk 0:b4b94eb28093 22
tks1 6:dd35cc3b5ca0 23 int Init_Pos = 0;
tks1 6:dd35cc3b5ca0 24 //超音波センサ関係パラメータ
tks1 6:dd35cc3b5ca0 25 int dist1;
tks1 6:dd35cc3b5ca0 26 int dist2;
tks1 6:dd35cc3b5ca0 27 int dist3;
Tomo1213 4:1bd08c9d92a9 28
ngokystk 0:b4b94eb28093 29 pc.printf("YUKA PROGRAM START\r\n");
ngokystk 0:b4b94eb28093 30 wait(0.1);
ngokystk 0:b4b94eb28093 31 //-------------起動時に必ず送信---------------
ngokystk 0:b4b94eb28093 32 //オペレーティングモードを送信
Tomo1213 3:c39c14cfc811 33 sendOPModeT(node1);
Tomo1213 3:c39c14cfc811 34 sendOPModeT(node2);
Tomo1213 3:c39c14cfc811 35 sendOPModeT(node3);
Tomo1213 3:c39c14cfc811 36 sendOPModeT(node4);
ngokystk 0:b4b94eb28093 37 //Shutdown,Enableコマンド送信｜リセット
ngokystk 0:b4b94eb28093 38 sendCtrlSD(node1);
ngokystk 0:b4b94eb28093 39 sendCtrlSD(node2);
ngokystk 0:b4b94eb28093 40 sendCtrlSD(node3);
ngokystk 0:b4b94eb28093 41 sendCtrlSD(node4);
tks1 6:dd35cc3b5ca0 42
ngokystk 0:b4b94eb28093 43 sendCtrlEN(node1);
ngokystk 0:b4b94eb28093 44 sendCtrlEN(node2);
ngokystk 0:b4b94eb28093 45 sendCtrlEN(node3);
ngokystk 0:b4b94eb28093 46 sendCtrlEN(node4);
tks1 6:dd35cc3b5ca0 47
ngokystk 0:b4b94eb28093 48 //初期加減速度
Tomo1213 3:c39c14cfc811 49 int Acc = 2000;
Tomo1213 3:c39c14cfc811 50 int Dec = 2000;
Tomo1213 3:c39c14cfc811 51 sendProAcc(1,Acc);
Tomo1213 3:c39c14cfc811 52 sendProAcc(2,Acc);
Tomo1213 3:c39c14cfc811 53 sendProAcc(3,Acc);
Tomo1213 3:c39c14cfc811 54 sendProAcc(4,Acc);
tks1 6:dd35cc3b5ca0 55
Tomo1213 3:c39c14cfc811 56 sendProDec(1,Dec);
Tomo1213 3:c39c14cfc811 57 sendProDec(2,Dec);
Tomo1213 3:c39c14cfc811 58 sendProDec(3,Dec);
tks1 6:dd35cc3b5ca0 59 sendProDec(4,Dec);
ngokystk 0:b4b94eb28093 60 //トルク設定
ngokystk 0:b4b94eb28093 61 int trq = 100; //torque Setting[mA]
tks1 6:dd35cc3b5ca0 62
ngokystk 0:b4b94eb28093 63 sendTgtTrq(1,trq);
ngokystk 0:b4b94eb28093 64 sendTgtTrq(2,trq);
ngokystk 0:b4b94eb28093 65 sendTgtTrq(3,trq);
ngokystk 0:b4b94eb28093 66 sendTgtTrq(4,trq);
tks1 6:dd35cc3b5ca0 67
Tomo1213 4:1bd08c9d92a9 68 int state_1 = 0;
tks1 6:dd35cc3b5ca0 69 int state_2 = 0;
Tomo1213 4:1bd08c9d92a9 70 int ride_count = 0;
Tomo1213 4:1bd08c9d92a9 71
Tomo1213 4:1bd08c9d92a9 72 int X_POS_TMP = 0;
Tomo1213 4:1bd08c9d92a9 73 int dist1_ori = 0;
Tomo1213 4:1bd08c9d92a9 74 int dist2_ori = 0;
Tomo1213 4:1bd08c9d92a9 75
Tomo1213 4:1bd08c9d92a9 76 readActPos(1);
Tomo1213 4:1bd08c9d92a9 77 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
tks1 6:dd35cc3b5ca0 78 if(ActPos > 8388608) {
Tomo1213 4:1bd08c9d92a9 79 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
Tomo1213 4:1bd08c9d92a9 80 }
Tomo1213 4:1bd08c9d92a9 81 Init_Pos = ActPos;//起動時の角度を保存
tks1 6:dd35cc3b5ca0 82 t_obj.reset();
tks1 6:dd35cc3b5ca0 83 t_obj.start();
Tomo1213 4:1bd08c9d92a9 84 //set_MODE_T();
Tomo1213 4:1bd08c9d92a9 85 printf("\nstart\r\n");
tks1 6:dd35cc3b5ca0 86 while(1) {
tks1 6:dd35cc3b5ca0 87 auto_time = t_obj.read();
Tomo1213 4:1bd08c9d92a9 88 pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2);
tks1 6:dd35cc3b5ca0 89 LED.printf("%d",state_1);
Tomo1213 1:b2bd1511307e 90 readActPos(1);
Tomo1213 1:b2bd1511307e 91 ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
tks1 6:dd35cc3b5ca0 92 if(ActPos > 8388608) {
Tomo1213 1:b2bd1511307e 93 ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
Tomo1213 4:1bd08c9d92a9 94 //printf("check\r\n");
ngokystk 0:b4b94eb28093 95 }
tks1 6:dd35cc3b5ca0 96 dist1 = get_cm_n(u1, 3);
tks1 6:dd35cc3b5ca0 97 dist2 = get_cm_n(u2, 3);
tks1 6:dd35cc3b5ca0 98 dist3 = get_cm_n(u3, 3);
Tomo1213 4:1bd08c9d92a9 99 /*--------------------------*/
tks1 6:dd35cc3b5ca0 100 if(state_1 == 0) { //入力判断フェーズ
Tomo1213 4:1bd08c9d92a9 101 state_2 = 0;
tks1 6:dd35cc3b5ca0 102 if(ride_count >= 2 && auto_time > Standby_Time) {
tks1 6:dd35cc3b5ca0 103 state_1 = 20;
tks1 6:dd35cc3b5ca0 104 } else {
tks1 6:dd35cc3b5ca0 105 if(ActPos < (Init_Pos - KICK)) { //前入力検出
tks1 6:dd35cc3b5ca0 106 ride_count++;
Tomo1213 4:1bd08c9d92a9 107 state_1 = 1;
tks1 6:dd35cc3b5ca0 108 } else if(ActPos > (Init_Pos + KICK)) { //右入力検出
Tomo1213 4:1bd08c9d92a9 109 ride_count++;
Tomo1213 4:1bd08c9d92a9 110 state_1 = 11;
tks1 6:dd35cc3b5ca0 111 } else {
Tomo1213 4:1bd08c9d92a9 112 set_MODE_T();
Tomo1213 1:b2bd1511307e 113 }
Tomo1213 4:1bd08c9d92a9 114 }
tks1 6:dd35cc3b5ca0 115
tks1 6:dd35cc3b5ca0 116 } else if(state_1 == 1) { //前進→壁検出フェーズ
tks1 6:dd35cc3b5ca0 117 if(dist1 < WALL && dist1 >= WALL_MIN) {
Tomo1213 4:1bd08c9d92a9 118 vel_stop();
Tomo1213 4:1bd08c9d92a9 119 wait(GETOFF_TIME);
Tomo1213 4:1bd08c9d92a9 120 state_1 = 2;
tks1 6:dd35cc3b5ca0 121 } else {
Tomo1213 4:1bd08c9d92a9 122 set_ACC(ACC_RIDE);//加速度設定
Tomo1213 4:1bd08c9d92a9 123 set_DEC(DEC_CLEAN);//減速度設定
Tomo1213 4:1bd08c9d92a9 124 set_MODE_V();//速度制御モード送信
Tomo1213 4:1bd08c9d92a9 125 vel_forward_con(RPM_RIDE);//前進速度指令
Tomo1213 4:1bd08c9d92a9 126 }
tks1 6:dd35cc3b5ca0 127 } else if(state_1 == 2) { //前進からの帰還フェーズ
tks1 6:dd35cc3b5ca0 128 if(ActPos > -3000) {
Tomo1213 4:1bd08c9d92a9 129 vel_stop();
tks1 6:dd35cc3b5ca0 130 t_obj.reset();
tks1 6:dd35cc3b5ca0 131 t_obj.start();
Tomo1213 4:1bd08c9d92a9 132 state_1 = 0;
Tomo1213 4:1bd08c9d92a9 133 wait(1.0);
tks1 6:dd35cc3b5ca0 134 } else {
Tomo1213 4:1bd08c9d92a9 135 vel_backward_con(RPM_RIDE);
Tomo1213 4:1bd08c9d92a9 136 }
tks1 6:dd35cc3b5ca0 137 } else if(state_1 == 11) { //右進→壁検出フェーズ
tks1 6:dd35cc3b5ca0 138 if(dist3 < WALL && dist3 >= WALL_MIN) {
Tomo1213 4:1bd08c9d92a9 139 vel_stop();
Tomo1213 4:1bd08c9d92a9 140 wait(GETOFF_TIME);
Tomo1213 4:1bd08c9d92a9 141 state_1 = 12;
tks1 6:dd35cc3b5ca0 142 } else {
Tomo1213 4:1bd08c9d92a9 143 set_ACC(ACC_RIDE);//加速度設定
Tomo1213 4:1bd08c9d92a9 144 set_DEC(DEC_RIDE);//減速度設定
Tomo1213 4:1bd08c9d92a9 145 set_MODE_V();//速度制御モード送信
Tomo1213 4:1bd08c9d92a9 146 vel_right(RPM_RIDE);//R進速度指令
Tomo1213 4:1bd08c9d92a9 147 }
tks1 6:dd35cc3b5ca0 148 } else if(state_1 == 12) { //右進からの帰還フェーズ
tks1 6:dd35cc3b5ca0 149 if(ActPos < 3000) {
Tomo1213 4:1bd08c9d92a9 150 vel_stop();
tks1 6:dd35cc3b5ca0 151 t_obj.reset();
tks1 6:dd35cc3b5ca0 152 t_obj.start();
Tomo1213 4:1bd08c9d92a9 153 state_1 = 0;
Tomo1213 4:1bd08c9d92a9 154 wait(1.0);
tks1 6:dd35cc3b5ca0 155 } else {
Tomo1213 4:1bd08c9d92a9 156 vel_left(RPM_RIDE);
tks1 6:dd35cc3b5ca0 157 }
tks1 6:dd35cc3b5ca0 158 } else if(state_1 == 20) { //消毒モード
tks1 6:dd35cc3b5ca0 159 if(state_2 == 0) {
tks1 6:dd35cc3b5ca0 160 if(dist1 < WALL && dist1 >= WALL_MIN) {
Tomo1213 4:1bd08c9d92a9 161 X_POS_TMP = ActPos;
Tomo1213 4:1bd08c9d92a9 162 state_2 = 1;
tks1 6:dd35cc3b5ca0 163 } else {
Tomo1213 4:1bd08c9d92a9 164 set_ACC(ACC_CLEAN);//加速度設定
Tomo1213 4:1bd08c9d92a9 165 set_DEC(DEC_CLEAN);//減速度設定
Tomo1213 4:1bd08c9d92a9 166 set_MODE_V();//速度制御モード送信
Tomo1213 4:1bd08c9d92a9 167 vel_forward_con(RPM_CLEAN);//前進速度指令
tks1 6:dd35cc3b5ca0 168 }
tks1 6:dd35cc3b5ca0 169 } else if(state_2 == 1) {
tks1 6:dd35cc3b5ca0 170 if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET) {
Tomo1213 4:1bd08c9d92a9 171 state_2 == 2;
tks1 6:dd35cc3b5ca0 172 } else {
tks1 6:dd35cc3b5ca0 173 if(dist3 < WALL && dist3 >= WALL_MIN) {
Tomo1213 4:1bd08c9d92a9 174 state_2 = 4;
tks1 6:dd35cc3b5ca0 175 } else {
Tomo1213 4:1bd08c9d92a9 176 set_ACC(ACC_CLEAN);//加速度設定
Tomo1213 4:1bd08c9d92a9 177 set_DEC(DEC_CLEAN);//減速度設定
Tomo1213 4:1bd08c9d92a9 178 set_MODE_V();//速度制御モード送信
Tomo1213 4:1bd08c9d92a9 179 vel_right(RPM_CLEAN);//右進速度指令
Tomo1213 1:b2bd1511307e 180 }
Tomo1213 4:1bd08c9d92a9 181 }
tks1 6:dd35cc3b5ca0 182 } else if(state_2 == 2) {
tks1 6:dd35cc3b5ca0 183 if(ActPos > -3000) {
Tomo1213 4:1bd08c9d92a9 184 state_2 = 3;
tks1 6:dd35cc3b5ca0 185 } else {
Tomo1213 4:1bd08c9d92a9 186 set_ACC(ACC_CLEAN);//加速度設定
Tomo1213 4:1bd08c9d92a9 187 set_DEC(DEC_CLEAN);//減速度設定
Tomo1213 4:1bd08c9d92a9 188 set_MODE_V();//速度制御モード送信
Tomo1213 4:1bd08c9d92a9 189 vel_backward_con(RPM_CLEAN);//後進速度指令
Tomo1213 4:1bd08c9d92a9 190 }
tks1 6:dd35cc3b5ca0 191 } else if(state_2 == 3) {
tks1 6:dd35cc3b5ca0 192 if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET) {
Tomo1213 4:1bd08c9d92a9 193 state_2 == 0;
tks1 6:dd35cc3b5ca0 194 } else {
tks1 6:dd35cc3b5ca0 195 if(dist3 < WALL && dist3 >= WALL_MIN) {
Tomo1213 4:1bd08c9d92a9 196 state_2 = 4;
tks1 6:dd35cc3b5ca0 197 } else {
tks1 6:dd35cc3b5ca0 198 set_ACC(ACC_CLEAN);//加速度設定
tks1 6:dd35cc3b5ca0 199 set_DEC(DEC_CLEAN);//減速度設定
tks1 6:dd35cc3b5ca0 200 set_MODE_V();//速度制御モード送信
tks1 6:dd35cc3b5ca0 201 vel_right(RPM_CLEAN);//右進速度指令
Tomo1213 1:b2bd1511307e 202 }
tks1 6:dd35cc3b5ca0 203 }
tks1 6:dd35cc3b5ca0 204 } else if(state_2 == 4) {
tks1 6:dd35cc3b5ca0 205 if(ActPos < 3000) {
Tomo1213 4:1bd08c9d92a9 206 state_2 = 5;
tks1 6:dd35cc3b5ca0 207 } else {
Tomo1213 4:1bd08c9d92a9 208 vel_left(RPM_CLEAN);
tks1 6:dd35cc3b5ca0 209 }
tks1 6:dd35cc3b5ca0 210 } else if(state_2 == 5) {
tks1 6:dd35cc3b5ca0 211 if(ActPos > -3000) {
tks1 6:dd35cc3b5ca0 212 t_obj.reset();
tks1 6:dd35cc3b5ca0 213 t_obj.start();
Tomo1213 4:1bd08c9d92a9 214 state_1 = 0;
Tomo1213 4:1bd08c9d92a9 215 state_2 = 0;
tks1 6:dd35cc3b5ca0 216 } else {
Tomo1213 4:1bd08c9d92a9 217 vel_backward_con(RPM_CLEAN);
Tomo1213 1:b2bd1511307e 218 }
Tomo1213 4:1bd08c9d92a9 219 }
tks1 6:dd35cc3b5ca0 220 }
Tomo1213 4:1bd08c9d92a9 221 }
Tomo1213 4:1bd08c9d92a9 222 }