taka yamanouchi / Mbed 2 deprecated 2019_tourobo_upper

0.5

Dependencies:   mbed PID QEI2 MotorDrivers Filter

main.cpp@0:c2f05c36e357, 2019-11-19 (annotated)

Committer:
sink
Date:
Tue Nov 19 09:43:25 2019 +0000
Revision:
0:c2f05c36e357
feild

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sink 0:c2f05c36e357 1 #include "mbed.h"
sink 0:c2f05c36e357 2 #include "define.h"
sink 0:c2f05c36e357 3 #include "Sabertooth_Serial.h"
sink 0:c2f05c36e357 4 #include "PID.h"
sink 0:c2f05c36e357 5 #include "QEI.h"
sink 0:c2f05c36e357 6 #include "Filter.h"
sink 0:c2f05c36e357 7 #include "string"
sink 0:c2f05c36e357 8
sink 0:c2f05c36e357 9 Ticker timer;
sink 0:c2f05c36e357 10 Timer T;
sink 0:c2f05c36e357 11
sink 0:c2f05c36e357 12 SaberSerial MD_S( 19200, p13, p14);
sink 0:c2f05c36e357 13 RawSerial Master( p9, p10, 115200);
sink 0:c2f05c36e357 14 RawSerial pc( USBTX, USBRX, 115200);
sink 0:c2f05c36e357 15
sink 0:c2f05c36e357 16 PID pid1(INT_TIME);
sink 0:c2f05c36e357 17 PID pid2(INT_TIME);
sink 0:c2f05c36e357 18
sink 0:c2f05c36e357 19 Filter LPF1(INT_TIME);
sink 0:c2f05c36e357 20 Filter LPF2(INT_TIME);
sink 0:c2f05c36e357 21
sink 0:c2f05c36e357 22 QEI Enc1(p18,p15,NC,RESO,&T,QEI::X4_ENCODING);
sink 0:c2f05c36e357 23 QEI Enc2(p17,p16,NC,RESO,&T,QEI::X4_ENCODING);
sink 0:c2f05c36e357 24 //_____________________________↓機能設定
sink 0:c2f05c36e357 25
sink 0:c2f05c36e357 26 DigitalIn sw1(p29);
sink 0:c2f05c36e357 27 DigitalIn sw2(p30);
sink 0:c2f05c36e357 28 //DigitalIn pe1(p19);
sink 0:c2f05c36e357 29 //DigitalIn pe2(p20);
sink 0:c2f05c36e357 30
sink 0:c2f05c36e357 31 DigitalOut fet1(p25);//CB回収
sink 0:c2f05c36e357 32 DigitalOut fet2(p26);//つっかえ
sink 0:c2f05c36e357 33 DigitalOut fet3(p27);//押出 上
sink 0:c2f05c36e357 34 DigitalOut fet4(p28);//押出 下
sink 0:c2f05c36e357 35
sink 0:c2f05c36e357 36 DigitalOut led1(LED1);
sink 0:c2f05c36e357 37 DigitalOut led2(LED2);
sink 0:c2f05c36e357 38 DigitalOut led3(LED3);
sink 0:c2f05c36e357 39 DigitalOut led4(LED4);
sink 0:c2f05c36e357 40 //_____________________________↓変数宣言
sink 0:c2f05c36e357 41
sink 0:c2f05c36e357 42 string Master_str = "";
sink 0:c2f05c36e357 43 char global_mode = 0b00000000;
sink 0:c2f05c36e357 44 int gl_enc1 = 0, gl_enc2 = 0;
sink 0:c2f05c36e357 45 double ref_spd1 = 0, ref_spd2 = 0, load_co = 0, recv_co = 0;
sink 0:c2f05c36e357 46 double f_spd1 = 0, f_spd2 = 0;
sink 0:c2f05c36e357 47 int gl_cmd1 = 0, gl_cmd2 = 0;
sink 0:c2f05c36e357 48 int gl_move = 0, gl_channel = 0;
sink 0:c2f05c36e357 49
sink 0:c2f05c36e357 50 void Master_send(char mode){
sink 0:c2f05c36e357 51 Master.printf("%c\n",mode);
sink 0:c2f05c36e357 52 }
sink 0:c2f05c36e357 53
sink 0:c2f05c36e357 54 //______________________________↓受信割込み
sink 0:c2f05c36e357 55
sink 0:c2f05c36e357 56 void Master_recv(){
sink 0:c2f05c36e357 57 recv_co = 1;
sink 0:c2f05c36e357 58 char Master_c = Master.getc();
sink 0:c2f05c36e357 59 static char local_mode = 0b00000000;
sink 0:c2f05c36e357 60
sink 0:c2f05c36e357 61 if(Master_c == '\n'){
sink 0:c2f05c36e357 62 local_mode = Master_str[0];
sink 0:c2f05c36e357 63 if(local_mode == EMERGENCY) {
sink 0:c2f05c36e357 64 global_mode = EMERGENCY;
sink 0:c2f05c36e357 65 }
sink 0:c2f05c36e357 66 else {
sink 0:c2f05c36e357 67 global_mode = local_mode;
sink 0:c2f05c36e357 68 }
sink 0:c2f05c36e357 69 Master_str = "";
sink 0:c2f05c36e357 70 }
sink 0:c2f05c36e357 71 else Master_str += Master_c;
sink 0:c2f05c36e357 72 }
sink 0:c2f05c36e357 73 //________________________________________↓速度制限・加減速制限
sink 0:c2f05c36e357 74
sink 0:c2f05c36e357 75 double limit_MD(double cmd,double max_cmd){
sink 0:c2f05c36e357 76 if (cmd > max_cmd) cmd = max_cmd;
sink 0:c2f05c36e357 77 else if (cmd < -max_cmd) cmd = -max_cmd;
sink 0:c2f05c36e357 78 return cmd;
sink 0:c2f05c36e357 79 }
sink 0:c2f05c36e357 80
sink 0:c2f05c36e357 81 //_____________________________↓主要処理(周期割込み)
sink 0:c2f05c36e357 82
sink 0:c2f05c36e357 83 void timer_warikomi(){
sink 0:c2f05c36e357 84 char mode = global_mode;
sink 0:c2f05c36e357 85 int sw1_def = sw1.read(), sw2_def = sw2.read();
sink 0:c2f05c36e357 86 int enc1 = Enc1.getPulses(), enc2 = Enc2.getPulses();
sink 0:c2f05c36e357 87 static char send_mode = 0b00000000;
sink 0:c2f05c36e357 88 static bool points1 = 0, points2 = 0, points3 = 0;
sink 0:c2f05c36e357 89 static bool positions = 0;
sink 0:c2f05c36e357 90 static bool pre_sw1 = 1, pre_sw2 = 1, move = 0, channel = 0;
sink 0:c2f05c36e357 91 static int counter = 0, point_co = 0, p1_co = 0 ,p3_co = 0, p2_co = 0;
sink 0:c2f05c36e357 92 static int pre_enc1 = 0, pre_enc2 = 0;
sink 0:c2f05c36e357 93 static double cmd1 = 0, cmd2 = 0, up_cmd = 0;
sink 0:c2f05c36e357 94
sink 0:c2f05c36e357 95
sink 0:c2f05c36e357 96 if(sw1_def == 0 && pre_sw1 == 1) move = !move;
sink 0:c2f05c36e357 97 if(sw2_def == 0 && pre_sw2 == 1) channel = !channel;
sink 0:c2f05c36e357 98
sink 0:c2f05c36e357 99 pre_sw1 = sw1_def;
sink 0:c2f05c36e357 100 pre_sw2 = sw2_def;
sink 0:c2f05c36e357 101
sink 0:c2f05c36e357 102 if(!recv_co);
sink 0:c2f05c36e357 103 if(mode == EMERGENCY){
sink 0:c2f05c36e357 104 f_spd1 = 0;
sink 0:c2f05c36e357 105 f_spd2 = 0;
sink 0:c2f05c36e357 106 up_cmd = 0;
sink 0:c2f05c36e357 107 if(counter >= EMER_COUNT){
sink 0:c2f05c36e357 108 led1 = !led1;
sink 0:c2f05c36e357 109 led2 = !led2;
sink 0:c2f05c36e357 110 led3 = !led3;
sink 0:c2f05c36e357 111 led4 = !led4;
sink 0:c2f05c36e357 112 counter = 0;
sink 0:c2f05c36e357 113 }else counter++;
sink 0:c2f05c36e357 114 }else if(mode & AUTOMATION){
sink 0:c2f05c36e357 115 up_cmd = 0;
sink 0:c2f05c36e357 116 led3 = 1;
sink 0:c2f05c36e357 117 led4 = 1;
sink 0:c2f05c36e357 118 points1 = (mode & POINTS_1);//20
sink 0:c2f05c36e357 119 points2 = (mode & POINTS_2);//40
sink 0:c2f05c36e357 120 points3 = (mode & POINTS_3);//10
sink 0:c2f05c36e357 121 positions = (mode & POSITIONS);
sink 0:c2f05c36e357 122
sink 0:c2f05c36e357 123 if(!points1 && !points2 && !points3){
sink 0:c2f05c36e357 124 f_spd1 = 0;
sink 0:c2f05c36e357 125 f_spd2 = 0;
sink 0:c2f05c36e357 126 }
sink 0:c2f05c36e357 127 if (points2){
sink 0:c2f05c36e357 128 led4 = 1;
sink 0:c2f05c36e357 129 f_spd1 = REF_SPD2_1;
sink 0:c2f05c36e357 130 f_spd2 = REF_SPD2_2;
sink 0:c2f05c36e357 131 send_mode |= POINTS_2;
sink 0:c2f05c36e357 132 if(positions && (enc1 - pre_enc1) > (REF_SPD2_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
sink 0:c2f05c36e357 133 if(p1_co < 3){
sink 0:c2f05c36e357 134 point_co++;
sink 0:c2f05c36e357 135 if(point_co >= 20){
sink 0:c2f05c36e357 136 fet3 = F3_ON;
sink 0:c2f05c36e357 137 p1_co++;
sink 0:c2f05c36e357 138 point_co = 0;
sink 0:c2f05c36e357 139 }else fet3 = F3_OFF;
sink 0:c2f05c36e357 140 }else{
sink 0:c2f05c36e357 141 send_mode |= POSITIONS;
sink 0:c2f05c36e357 142 fet3 = F3_OFF;
sink 0:c2f05c36e357 143 }
sink 0:c2f05c36e357 144 }else{
sink 0:c2f05c36e357 145 send_mode &= ~POINTS_1;
sink 0:c2f05c36e357 146 send_mode &= ~POSITIONS;
sink 0:c2f05c36e357 147 }
sink 0:c2f05c36e357 148 }if(points1){
sink 0:c2f05c36e357 149 led4 = 1;
sink 0:c2f05c36e357 150 f_spd1 = REF_SPD1_1;
sink 0:c2f05c36e357 151 f_spd2 = REF_SPD1_2;
sink 0:c2f05c36e357 152 send_mode |= POINTS_1;
sink 0:c2f05c36e357 153 if(positions && (enc1 - pre_enc1) > (REF_SPD1_1 - 30) && (enc1 - pre_enc1) < (REF_SPD1_1 + 30)){
sink 0:c2f05c36e357 154 fet3 = F3_ON;
sink 0:c2f05c36e357 155 if(p2_co > 8) send_mode |= POSITIONS;
sink 0:c2f05c36e357 156 else p2_co++;
sink 0:c2f05c36e357 157 }else{
sink 0:c2f05c36e357 158 fet3 = F3_OFF;
sink 0:c2f05c36e357 159 send_mode &= ~POINTS_3;
sink 0:c2f05c36e357 160 send_mode &= ~POSITIONS;
sink 0:c2f05c36e357 161 }
sink 0:c2f05c36e357 162 }if(points3){
sink 0:c2f05c36e357 163 led4 = 1;
sink 0:c2f05c36e357 164 fet2 = F2_OFF;
sink 0:c2f05c36e357 165 fet3 = F3_OFF;
sink 0:c2f05c36e357 166 f_spd1 = REF_SPD3_1;
sink 0:c2f05c36e357 167 f_spd2 = REF_SPD3_2;
sink 0:c2f05c36e357 168 send_mode |= POINTS_3;
sink 0:c2f05c36e357 169 if(positions && (enc1 - pre_enc1) > (REF_SPD3_1 - 30) && (enc1 - pre_enc1) < (REF_SPD2_1 + 30)){
sink 0:c2f05c36e357 170 if(p3_co < 6){
sink 0:c2f05c36e357 171 if (point_co >= 20){
sink 0:c2f05c36e357 172 fet4 = F4_ON;
sink 0:c2f05c36e357 173 point_co = 0;
sink 0:c2f05c36e357 174 p3_co++;
sink 0:c2f05c36e357 175 }else {
sink 0:c2f05c36e357 176 fet4 = F4_OFF;
sink 0:c2f05c36e357 177 send_mode &= ~POINTS_2;
sink 0:c2f05c36e357 178 send_mode &= ~POSITIONS;
sink 0:c2f05c36e357 179 }
sink 0:c2f05c36e357 180 }else{
sink 0:c2f05c36e357 181 if(p3_co > 15){
sink 0:c2f05c36e357 182 fet4 = F4_OFF;
sink 0:c2f05c36e357 183 send_mode |= POSITIONS;
sink 0:c2f05c36e357 184 f_spd1 = 0;
sink 0:c2f05c36e357 185 f_spd2 = 0;
sink 0:c2f05c36e357 186 }else p3_co++;
sink 0:c2f05c36e357 187 }
sink 0:c2f05c36e357 188 }
sink 0:c2f05c36e357 189 }else if(positions){
sink 0:c2f05c36e357 190 led4 = 1;
sink 0:c2f05c36e357 191 send_mode |= POSITIONS;
sink 0:c2f05c36e357 192 }else if(!positions){
sink 0:c2f05c36e357 193 send_mode &= ~POSITIONS;
sink 0:c2f05c36e357 194 }else;
sink 0:c2f05c36e357 195
sink 0:c2f05c36e357 196 Master_send(send_mode);
sink 0:c2f05c36e357 197 }else{
sink 0:c2f05c36e357 198 /*led1 = 0;
sink 0:c2f05c36e357 199 led2 = 0;
sink 0:c2f05c36e357 200 led3 = 0;
sink 0:c2f05c36e357 201 led4 = 0;*/
sink 0:c2f05c36e357 202 if(mode & CB_COLLECT){
sink 0:c2f05c36e357 203 up_cmd = 50;
sink 0:c2f05c36e357 204 fet1 = F1_ON;
sink 0:c2f05c36e357 205 }else{
sink 0:c2f05c36e357 206 fet1 = F1_OFF;
sink 0:c2f05c36e357 207 up_cmd = 0;
sink 0:c2f05c36e357 208 }
sink 0:c2f05c36e357 209 if(move){
sink 0:c2f05c36e357 210 led1 = 1;
sink 0:c2f05c36e357 211 led2 = 0;
sink 0:c2f05c36e357 212 f_spd1 = REF_SPD3_1;
sink 0:c2f05c36e357 213 f_spd2 = REF_SPD3_2;
sink 0:c2f05c36e357 214 if(channel) fet4 = F4_ON;
sink 0:c2f05c36e357 215 else fet4 = F4_OFF;
sink 0:c2f05c36e357 216 }else{
sink 0:c2f05c36e357 217 led1 = 0;
sink 0:c2f05c36e357 218 led2 = 1;
sink 0:c2f05c36e357 219 f_spd1 = 0;
sink 0:c2f05c36e357 220 f_spd2 = 0;
sink 0:c2f05c36e357 221 if(channel) fet4 = F4_ON;
sink 0:c2f05c36e357 222 else fet4 = F4_OFF;
sink 0:c2f05c36e357 223 }
sink 0:c2f05c36e357 224 }
sink 0:c2f05c36e357 225 gl_enc1 = enc1 - pre_enc1;
sink 0:c2f05c36e357 226 gl_enc2 = enc2 - pre_enc2;
sink 0:c2f05c36e357 227
sink 0:c2f05c36e357 228 ref_spd1 = LPF1.LowPassFilter(f_spd1);
sink 0:c2f05c36e357 229 ref_spd2 = LPF2.LowPassFilter(f_spd2);
sink 0:c2f05c36e357 230
sink 0:c2f05c36e357 231 cmd1 += pid1.con(ref_spd1 - gl_enc1);
sink 0:c2f05c36e357 232 cmd1 = limit_MD( cmd1, 120);
sink 0:c2f05c36e357 233 if(f_spd1 == 0 && abs(cmd1) < 20) cmd1 = 0;
sink 0:c2f05c36e357 234 if(cmd1 >= 0) MD_S.Serial(SB_ADRS1, 1, (int)(abs(cmd1)));
sink 0:c2f05c36e357 235 else MD_S.Serial(SB_ADRS1, 0, (int)(abs(cmd1)));
sink 0:c2f05c36e357 236
sink 0:c2f05c36e357 237 cmd2 += pid2.con(ref_spd2 - gl_enc2);
sink 0:c2f05c36e357 238 cmd2 = limit_MD( cmd2, 120);
sink 0:c2f05c36e357 239 if(f_spd2 == 0 && abs(cmd2) < 20) cmd2 = 0;
sink 0:c2f05c36e357 240 if(cmd2 >= 0) MD_S.Serial(SB_ADRS1, 5, (int)(abs(cmd2)));
sink 0:c2f05c36e357 241 else MD_S.Serial(SB_ADRS1, 6, (int)(abs(cmd2)));
sink 0:c2f05c36e357 242
sink 0:c2f05c36e357 243 pre_enc1 = enc1;
sink 0:c2f05c36e357 244 pre_enc2 = enc2;
sink 0:c2f05c36e357 245
sink 0:c2f05c36e357 246 gl_cmd1 = cmd1;
sink 0:c2f05c36e357 247 gl_cmd2 = cmd2;
sink 0:c2f05c36e357 248 gl_move = move;
sink 0:c2f05c36e357 249 gl_channel = channel;
sink 0:c2f05c36e357 250
sink 0:c2f05c36e357 251 MD_S.Serial(SB_ADRS2, 5, up_cmd);
sink 0:c2f05c36e357 252 }
sink 0:c2f05c36e357 253 //__________________________________________
sink 0:c2f05c36e357 254
sink 0:c2f05c36e357 255 int main() {
sink 0:c2f05c36e357 256 fet1 = F1_OFF;
sink 0:c2f05c36e357 257 fet2 = F2_OFF;
sink 0:c2f05c36e357 258 fet3 = F3_OFF;
sink 0:c2f05c36e357 259 fet4 = F4_ON;
sink 0:c2f05c36e357 260 led1 = 0;
sink 0:c2f05c36e357 261 led2 = 0;
sink 0:c2f05c36e357 262 led3 = 0;
sink 0:c2f05c36e357 263 led4 = 0;
sink 0:c2f05c36e357 264
sink 0:c2f05c36e357 265 pid1.set(P1,NU,D1);
sink 0:c2f05c36e357 266 pid2.set(P2,NU,D1);
sink 0:c2f05c36e357 267
sink 0:c2f05c36e357 268 LPF1.setLowPassPara(2.0, 0);
sink 0:c2f05c36e357 269 LPF2.setLowPassPara(2.0, 0);
sink 0:c2f05c36e357 270
sink 0:c2f05c36e357 271 timer.attach(&timer_warikomi,INT_TIME);
sink 0:c2f05c36e357 272 Master.attach(&Master_recv ,RawSerial::RxIrq);
sink 0:c2f05c36e357 273 while(true) {
sink 0:c2f05c36e357 274 /*static int buff1 = 0, buff2 = 0;
sink 0:c2f05c36e357 275 buff1 = Enc1.getPulses();
sink 0:c2f05c36e357 276 buff2 = Enc2.getPulses();*/
sink 0:c2f05c36e357 277 pc.printf("ERR1:%d ERR2:%d CMD1:%d CMD2:%d MODE:%d CAHN:%d\n", gl_enc1, gl_enc2, gl_cmd1, gl_cmd2, int(global_mode), gl_channel);
sink 0:c2f05c36e357 278 wait(0.1);
sink 0:c2f05c36e357 279 }
sink 0:c2f05c36e357 280 }