![](/media/cache/profiles/sirooni.jpg.50x50_q85.jpg)
wheel_test7
Dependencies: QEI omni_wheel prototype01 PID ikarashiMDC OmniPosition
main.cpp
- Committer:
- piroro4560
- Date:
- 2019-08-22
- Revision:
- 0:898471aea1af
- Child:
- 1:93f6f64ac719
File content as of revision 0:898471aea1af:
#include "mbed.h" //wheel_test7 #include "ikarashiMDC.h" //sampleライブラリ+PID -> うひょひょひょ #include "omni_wheel.h" #include "sample02.h" #include "OmniPosition.h" #include "PID.h" PID angle(2.0,0.0,0.00001,0.01); OmniWheel omni(4); OmniPosition posi(PC_12, PD_2); Serial serial(PC_6, PC_7, 115200); DigitalOut serialcontrol(D10); RawSerial pc(USBTX, USBRX, 115200); DigitalOut sw1(PC_2); DigitalOut sw2(PC_3); ikarashiMDC motor[]= { ikarashiMDC(&serialcontrol,0,0,SM,&serial), ikarashiMDC(&serialcontrol,0,1,SM,&serial), ikarashiMDC(&serialcontrol,0,2,SM,&serial), ikarashiMDC(&serialcontrol,0,3,SM,&serial), }; Sample sample(500, 1000, 0.4, 0.14); int main() { sw1 = 1; sw2 = 2; int cnt=0; motor[0].braking = true; double valueX, valueY, value[4]; omni.wheel[0].setRadian(PI * 1 / 4); omni.wheel[1].setRadian(PI * 3 / 4); omni.wheel[2].setRadian(PI * 5 / 4); omni.wheel[3].setRadian(PI * 7 / 4); angle.setInputLimits(-3.14,3.14); angle.setOutputLimits(-0.4,0.4); angle.setBias(0); angle.setMode(1); angle.setSetPoint(0); while(1){ if (cnt==500) sample.target_xy(-3800, 0, posi.getX(), posi.getY()); if (cnt==1000) sample.target_xy(0, 0, posi.getX(), posi.getY()); if (cnt==1500) sample.target_xy(-2000, 0, posi.getX(), posi.getY()); if (cnt==2000) sample.target_xy(-2000, 2700, posi.getX(), posi.getY()); if (cnt==2500) sample.target_xy(-2000, 0, posi.getX(), posi.getY()); if (cnt==3000) sample.target_xy(0, 0, posi.getX(), posi.getY()); angle.setProcessValue(posi.getTheta()); double spin_power = -angle.compute(); sample.calculate(posi.getX(), posi.getY()); omni.computeXY(sample.getvalue_x(), sample.getvalue_y(), spin_power); // omni.computeXY(0, 0, spin_power); for(int i = 0; i < 4; i++){ value[i] = omni.wheel[i]; pc.printf("%.2f || ",value[i]); } int a = sample.nowDis > (sample.accdis+sample.middledis); pc.printf("x:%d || y:%d || θ:%f || vector:%f || now:%f || c:%f" , posi.getX(), posi.getY(), posi.getTheta(), sample.vector, sample.nowDis, sample.counter); for(int i = 0; i < 4; i++) { if (fabs(value[i]) < 0.05) value[i] = 0; motor[i].setSpeed(value[i]); } cnt++; pc.printf("||pid:%.2f\r\n",spin_power); } }