project-R / Mbed 2 deprecated TOUTEKI_all_mbed

TOUTEKI

Dependencies:   mbed QEI2 UnderBody Filter

main.cpp@7:24a3e797e7a8, 2019-03-11 (annotated)

Committer:
sink
Date:
Mon Mar 11 04:05:57 2019 +0000
Revision:
7:24a3e797e7a8
Parent:
6:7afdc6a81566 
ver2.1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5118069 0:d46cb1df87f1 1 #include "mbed.h"
e5118069 0:d46cb1df87f1 2 #include "QEI.h"
e5118069 1:94e15665b69f 3 #include "Filter.h"
sink 6:7afdc6a81566 4 #include "string"
sink 6:7afdc6a81566 5 #include "define.h"
sink 6:7afdc6a81566 6 #include "RoboClaw.h"
e5118069 0:d46cb1df87f1 7
e5118069 0:d46cb1df87f1 8 Ticker timer;
e5118069 0:d46cb1df87f1 9 Timer T;
sink 7:24a3e797e7a8 10
sink 6:7afdc6a81566 11 RoboClaw MD(115200,p9,p10);
e5118069 3:de0b5dc55627 12 Serial Saber(p13,p14);
e5118069 3:de0b5dc55627 13 Serial pc(USBTX,USBRX);
sink 6:7afdc6a81566 14 RawSerial Master(p28,p27,115200);//(tx,rx,baud);
e5118069 1:94e15665b69f 15 Filter velfilter(INT_TIME);
e5118069 0:d46cb1df87f1 16
sink 6:7afdc6a81566 17 DigitalOut fet1(p22);//shagai把持
sink 6:7afdc6a81566 18 DigitalOut fet2(p21);//shagai押出
sink 6:7afdc6a81566 19 DigitalOut fet3(p23);//shagaiハンド昇降
e5118069 0:d46cb1df87f1 20
sink 6:7afdc6a81566 21 DigitalIn limit1(p15);//shagaiハンドlimit
e5118069 2:965cba546262 22 DigitalIn limit2(p16);
sink 6:7afdc6a81566 23 //SENSのどっちか減るかも
sink 6:7afdc6a81566 24 DigitalIn SENS1(p18);//shagai検出
e5118069 1:94e15665b69f 25 DigitalIn SENS2(p17);
e5118069 1:94e15665b69f 26
sink 6:7afdc6a81566 27 DigitalIn G_limit1(p9);//gerege limit
e5118069 5:869dc702b923 28 DigitalIn G_limit2(p10);
e5118069 5:869dc702b923 29
e5118069 2:965cba546262 30 int cmd,A;
sink 7:24a3e797e7a8 31 int LIM1,LIM2;
e5118069 2:965cba546262 32 int S1,S2;
e5118069 1:94e15665b69f 33
sink 6:7afdc6a81566 34 char mode = 0x00;
e5118069 0:d46cb1df87f1 35
sink 6:7afdc6a81566 36 double filtered_ref_qpps;
e5118069 0:d46cb1df87f1 37
e5118069 5:869dc702b923 38 int G_LIM1=0,G_LIM2=0;
e5118069 5:869dc702b923 39
e5118069 5:869dc702b923 40 int G_cmd;
e5118069 5:869dc702b923 41
sink 6:7afdc6a81566 42 int limit_MD(int qpps,int max_qpps){
sink 6:7afdc6a81566 43 if (qpps > max_qpps) qpps = max_qpps;
sink 6:7afdc6a81566 44 else if (-qpps < -max_qpps) qpps = -max_qpps;
sink 6:7afdc6a81566 45 return qpps;
sink 6:7afdc6a81566 46 }
e5118069 5:869dc702b923 47
sink 6:7afdc6a81566 48 void robo_serial(int adrs, int qpps1, int qpps2){
sink 6:7afdc6a81566 49 MD.SpeedM1(adrs,qpps1);
sink 6:7afdc6a81566 50 MD.SpeedM2(adrs,qpps2);
e5118069 0:d46cb1df87f1 51 }
sink 6:7afdc6a81566 52
sink 6:7afdc6a81566 53 void Saber_Serial (int adrs, int rot, int cmd){
sink 6:7afdc6a81566 54 Saber.putc(adrs);
sink 6:7afdc6a81566 55 Saber.putc(rot);
sink 6:7afdc6a81566 56 Saber.putc(abs(cmd));
sink 6:7afdc6a81566 57 Saber.putc((adrs + rot + abs(cmd)) & 0b01111111);
e5118069 5:869dc702b923 58 }
e5118069 5:869dc702b923 59
sink 6:7afdc6a81566 60 /*void Slave_tx(char tx_mode){ //処理の終了を送る
sink 6:7afdc6a81566 61 Master.printf("%c",tx_mode);
sink 6:7afdc6a81566 62 Master.printf("%c",tx_mode);
sink 6:7afdc6a81566 63 Master.printf("\n");
sink 6:7afdc6a81566 64 }*/
sink 6:7afdc6a81566 65
sink 6:7afdc6a81566 66 void Slave_rx() { //処理内容を受け取る
sink 6:7afdc6a81566 67 static string rx_mode = "";
sink 6:7afdc6a81566 68 char rx_c = Master.getc();
sink 6:7afdc6a81566 69
sink 6:7afdc6a81566 70 if (rx_c == '\n') {
sink 6:7afdc6a81566 71 if (rx_mode.size() == 2){
sink 6:7afdc6a81566 72 if (rx_mode[0] == rx_mode[1]){
sink 6:7afdc6a81566 73 mode = rx_mode[1]; //モード切替
sink 6:7afdc6a81566 74 }}
sink 6:7afdc6a81566 75 rx_mode = "";
sink 6:7afdc6a81566 76 }
sink 6:7afdc6a81566 77
sink 6:7afdc6a81566 78 else {
sink 6:7afdc6a81566 79 if (rx_mode.size() > 2) rx_mode = "";
sink 6:7afdc6a81566 80 rx_mode += rx_c;
sink 6:7afdc6a81566 81 }
sink 6:7afdc6a81566 82 //pc.printf("%x\n",mode);
sink 6:7afdc6a81566 83 }
e5118069 0:d46cb1df87f1 84
e5118069 0:d46cb1df87f1 85 void timer_warikomi()
e5118069 1:94e15665b69f 86 {
sink 6:7afdc6a81566 87 static int qpps1 = 0;
sink 6:7afdc6a81566 88 static int qpps2 = 0;
sink 6:7afdc6a81566 89 static int ref_qpps1 = 0;
sink 6:7afdc6a81566 90 static int ref_qpps2 = 0;
e5118069 1:94e15665b69f 91
e5118069 2:965cba546262 92 LIM1=!limit1.read();
e5118069 2:965cba546262 93 LIM2=!limit2.read();
e5118069 2:965cba546262 94 S1=SENS1.read();
e5118069 0:d46cb1df87f1 95 S2=SENS2.read();
sink 6:7afdc6a81566 96 static char slave_mode = 0x00;
sink 6:7afdc6a81566 97 static int spd_count = 0;
e5118069 3:de0b5dc55627 98
sink 6:7afdc6a81566 99 G_LIM1=!G_limit1.read();//pullupなので逆
sink 6:7afdc6a81566 100 G_LIM2=!G_limit2.read();//pullupなので逆
sink 6:7afdc6a81566 101
sink 6:7afdc6a81566 102 switch (mode) {
sink 6:7afdc6a81566 103 case 0x99:
e5118069 0:d46cb1df87f1 104 break;
sink 6:7afdc6a81566 105
sink 6:7afdc6a81566 106 case 0x10://初期化処理
sink 6:7afdc6a81566 107 ref_qpps1 = 0.0;
sink 6:7afdc6a81566 108 ref_qpps2 = 0.0;
sink 6:7afdc6a81566 109
sink 6:7afdc6a81566 110 break;
sink 6:7afdc6a81566 111
sink 6:7afdc6a81566 112 case 0x20://ハンド降下
sink 6:7afdc6a81566 113 //goal_D=125;
e5118069 0:d46cb1df87f1 114 if(S1==0&&S2==0){
e5118069 3:de0b5dc55627 115 fet1=1;
e5118069 0:d46cb1df87f1 116 A=1;
e5118069 0:d46cb1df87f1 117 }
sink 6:7afdc6a81566 118 break;
sink 6:7afdc6a81566 119
sink 6:7afdc6a81566 120 case 0x30://ハンド上昇
sink 6:7afdc6a81566 121
sink 6:7afdc6a81566 122 break;
sink 6:7afdc6a81566 123
sink 6:7afdc6a81566 124 case 0x40://geregeハンド
sink 6:7afdc6a81566 125 G_cmd=120;
sink 6:7afdc6a81566 126 if(G_LIM1){
sink 6:7afdc6a81566 127 G_cmd=0;
sink 6:7afdc6a81566 128 }
sink 6:7afdc6a81566 129 break;
e5118069 0:d46cb1df87f1 130
sink 6:7afdc6a81566 131 case 0x50://geregeハンド
sink 6:7afdc6a81566 132 G_cmd=-120;
sink 6:7afdc6a81566 133 if(G_LIM2){
sink 6:7afdc6a81566 134 G_cmd=0;
sink 6:7afdc6a81566 135 slave_mode = 0x55;
sink 6:7afdc6a81566 136 }
sink 6:7afdc6a81566 137 break;
sink 6:7afdc6a81566 138
sink 6:7afdc6a81566 139 case 0x60://shagai発射
sink 6:7afdc6a81566 140 ref_qpps1 = REF_QPPS1;
sink 6:7afdc6a81566 141 ref_qpps2 = REF_QPPS2;
sink 6:7afdc6a81566 142 if (qpps1 == REF_QPPS1 && qpps2 == REF_QPPS2) {
sink 6:7afdc6a81566 143 spd_count++;
e5118069 0:d46cb1df87f1 144 }
e5118069 0:d46cb1df87f1 145 break;
sink 6:7afdc6a81566 146
sink 6:7afdc6a81566 147 case 0x90://全停止
sink 6:7afdc6a81566 148 ref_qpps1 = 0.0;
sink 6:7afdc6a81566 149 ref_qpps2 = 0.0;
sink 6:7afdc6a81566 150 G_cmd = 0;
e5118069 1:94e15665b69f 151 break;
sink 6:7afdc6a81566 152
sink 6:7afdc6a81566 153 default://何もしない
sink 6:7afdc6a81566 154 break;
sink 6:7afdc6a81566 155 }
e5118069 5:869dc702b923 156
sink 6:7afdc6a81566 157 //Slave_tx(slave_mode);
sink 6:7afdc6a81566 158 //二次遅れ系
sink 6:7afdc6a81566 159 if(qpps1>=QPPS_HIGH_RANGE1 && ref_qpps1 == REF_QPPS1) qpps1=ref_qpps1;
sink 6:7afdc6a81566 160 else if(qpps1<=QPPS_HIGH_STOP && ref_qpps1 == 0) qpps1=0;
sink 6:7afdc6a81566 161 else qpps1 = velfilter.SecondOrderLag((double)ref_qpps1);
sink 6:7afdc6a81566 162 if(qpps2>=QPPS_HIGH_RANGE2 && ref_qpps2 == REF_QPPS2) qpps2=ref_qpps2;
sink 6:7afdc6a81566 163 else if(qpps2<=QPPS_HIGH_STOP && ref_qpps2 == 0) qpps2=0;
sink 6:7afdc6a81566 164 else qpps2 = velfilter.SecondOrderLag((double)ref_qpps2);
sink 6:7afdc6a81566 165 //ローラー強制停止
sink 6:7afdc6a81566 166 /*if (qpps <= 5.0 && ref_qpps == 0) {
sink 6:7afdc6a81566 167 cmd3 = 0;
sink 6:7afdc6a81566 168 }*/
sink 6:7afdc6a81566 169 //自動押し出し
sink 6:7afdc6a81566 170 if (spd_count >= 40){
sink 6:7afdc6a81566 171 fet2 = 0;
sink 6:7afdc6a81566 172 if (slave_mode != 0x60) spd_count = 0;
sink 6:7afdc6a81566 173 }
e5118069 5:869dc702b923 174
sink 6:7afdc6a81566 175 qpps1 = limit_MD( qpps1, MAX_QPPS1);
sink 6:7afdc6a81566 176 qpps2 = limit_MD( qpps2, MAX_QPPS2);
e5118069 0:d46cb1df87f1 177
sink 6:7afdc6a81566 178 robo_serial(ROBO_ADRS, qpps1, qpps2);
e5118069 0:d46cb1df87f1 179
sink 6:7afdc6a81566 180 if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd);
sink 6:7afdc6a81566 181 else Saber_Serial (SABER_ADDR, 5, G_cmd);
sink 6:7afdc6a81566 182
e5118069 0:d46cb1df87f1 183 }
e5118069 0:d46cb1df87f1 184
e5118069 0:d46cb1df87f1 185
e5118069 0:d46cb1df87f1 186 int main() {
e5118069 1:94e15665b69f 187 Saber.baud(19200);
e5118069 0:d46cb1df87f1 188 pc.baud(9600);
e5118069 3:de0b5dc55627 189 fet1=0;
e5118069 2:965cba546262 190 fet2=1;
sink 6:7afdc6a81566 191 fet3=0;
e5118069 2:965cba546262 192
e5118069 5:869dc702b923 193 G_limit1.mode( PullUp ); // 内蔵プルアップを使う
e5118069 5:869dc702b923 194 G_limit2.mode( PullUp );
e5118069 5:869dc702b923 195
e5118069 1:94e15665b69f 196 velfilter.setSecondOrderPara(1.0, 0.9, 0.0);
e5118069 1:94e15665b69f 197
sink 6:7afdc6a81566 198 Master.attach(&Slave_rx, RawSerial::RxIrq);
e5118069 0:d46cb1df87f1 199 timer.attach(timer_warikomi,INT_TIME);
e5118069 0:d46cb1df87f1 200
e5118069 0:d46cb1df87f1 201 while(1) {
sink 6:7afdc6a81566 202 pc.printf("%lf\n", filtered_ref_qpps);
e5118069 0:d46cb1df87f1 203 }
e5118069 1:94e15665b69f 204 }