Ryu Anzawa / Mbed OS wheel_test7

wheel_test7

Dependencies:   QEI omni_wheel prototype01 PID ikarashiMDC OmniPosition

main.cpp@0:898471aea1af, 2019-08-22 (annotated)

Committer:
piroro4560
Date:
Thu Aug 22 06:39:31 2019 +0000
Revision:
0:898471aea1af
Child:
1:93f6f64ac719
gg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piroro4560 0:898471aea1af 1 #include "mbed.h" //wheel_test7
piroro4560 0:898471aea1af 2 #include "ikarashiMDC.h" //sampleライブラリ+PID -> うひょひょひょ
piroro4560 0:898471aea1af 3 #include "omni_wheel.h"
piroro4560 0:898471aea1af 4 #include "sample02.h"
piroro4560 0:898471aea1af 5 #include "OmniPosition.h"
piroro4560 0:898471aea1af 6 #include "PID.h"
piroro4560 0:898471aea1af 7
piroro4560 0:898471aea1af 8 PID angle(2.0,0.0,0.00001,0.01);
piroro4560 0:898471aea1af 9 OmniWheel omni(4);
piroro4560 0:898471aea1af 10 OmniPosition posi(PC_12, PD_2);
piroro4560 0:898471aea1af 11 Serial serial(PC_6, PC_7, 115200);
piroro4560 0:898471aea1af 12 DigitalOut serialcontrol(D10);
piroro4560 0:898471aea1af 13 RawSerial pc(USBTX, USBRX, 115200);
piroro4560 0:898471aea1af 14 DigitalOut sw1(PC_2);
piroro4560 0:898471aea1af 15 DigitalOut sw2(PC_3);
piroro4560 0:898471aea1af 16
piroro4560 0:898471aea1af 17 ikarashiMDC motor[]=
piroro4560 0:898471aea1af 18 {
piroro4560 0:898471aea1af 19 ikarashiMDC(&serialcontrol,0,0,SM,&serial),
piroro4560 0:898471aea1af 20 ikarashiMDC(&serialcontrol,0,1,SM,&serial),
piroro4560 0:898471aea1af 21 ikarashiMDC(&serialcontrol,0,2,SM,&serial),
piroro4560 0:898471aea1af 22 ikarashiMDC(&serialcontrol,0,3,SM,&serial),
piroro4560 0:898471aea1af 23 };
piroro4560 0:898471aea1af 24
piroro4560 0:898471aea1af 25 Sample sample(500, 1000, 0.4, 0.14);
piroro4560 0:898471aea1af 26
piroro4560 0:898471aea1af 27 int main()
piroro4560 0:898471aea1af 28 {
piroro4560 0:898471aea1af 29 sw1 = 1;
piroro4560 0:898471aea1af 30 sw2 = 2;
piroro4560 0:898471aea1af 31 int cnt=0;
piroro4560 0:898471aea1af 32 motor[0].braking = true;
piroro4560 0:898471aea1af 33 double valueX, valueY, value[4];
piroro4560 0:898471aea1af 34 omni.wheel[0].setRadian(PI * 1 / 4);
piroro4560 0:898471aea1af 35 omni.wheel[1].setRadian(PI * 3 / 4);
piroro4560 0:898471aea1af 36 omni.wheel[2].setRadian(PI * 5 / 4);
piroro4560 0:898471aea1af 37 omni.wheel[3].setRadian(PI * 7 / 4);
piroro4560 0:898471aea1af 38 angle.setInputLimits(-3.14,3.14);
piroro4560 0:898471aea1af 39 angle.setOutputLimits(-0.4,0.4);
piroro4560 0:898471aea1af 40 angle.setBias(0);
piroro4560 0:898471aea1af 41 angle.setMode(1);
piroro4560 0:898471aea1af 42 angle.setSetPoint(0);
piroro4560 0:898471aea1af 43 while(1){
piroro4560 0:898471aea1af 44
piroro4560 0:898471aea1af 45 if (cnt==500) sample.target_xy(-3800, 0, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 46 if (cnt==1000) sample.target_xy(0, 0, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 47 if (cnt==1500) sample.target_xy(-2000, 0, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 48 if (cnt==2000) sample.target_xy(-2000, 2700, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 49 if (cnt==2500) sample.target_xy(-2000, 0, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 50 if (cnt==3000) sample.target_xy(0, 0, posi.getX(), posi.getY());
piroro4560 0:898471aea1af 51 angle.setProcessValue(posi.getTheta());
piroro4560 0:898471aea1af 52 double spin_power = -angle.compute();
piroro4560 0:898471aea1af 53 sample.calculate(posi.getX(), posi.getY());
piroro4560 0:898471aea1af 54 omni.computeXY(sample.getvalue_x(), sample.getvalue_y(), spin_power);
piroro4560 0:898471aea1af 55 // omni.computeXY(0, 0, spin_power);
piroro4560 0:898471aea1af 56
piroro4560 0:898471aea1af 57 for(int i = 0; i < 4; i++){
piroro4560 0:898471aea1af 58 value[i] = omni.wheel[i];
piroro4560 0:898471aea1af 59 pc.printf("%.2f || ",value[i]);
piroro4560 0:898471aea1af 60 }
piroro4560 0:898471aea1af 61 int a = sample.nowDis > (sample.accdis+sample.middledis);
piroro4560 0:898471aea1af 62 pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f"
piroro4560 0:898471aea1af 63 , posi.getX(), posi.getY(), posi.getTheta(), sample.vector, sample.nowDis, sample.counter);
piroro4560 0:898471aea1af 64
piroro4560 0:898471aea1af 65 for(int i = 0; i < 4; i++) {
piroro4560 0:898471aea1af 66 if (fabs(value[i]) < 0.05) value[i] = 0;
piroro4560 0:898471aea1af 67 motor[i].setSpeed(value[i]);
piroro4560 0:898471aea1af 68 }
piroro4560 0:898471aea1af 69 cnt++;
piroro4560 0:898471aea1af 70 pc.printf("||pid:%.2f\r\n",spin_power);
piroro4560 0:898471aea1af 71 }
piroro4560 0:898471aea1af 72 }