wheel_test4_ver1
Dependencies: QEI omni_wheel PID R1370 Controller ikarashiMDC
main.cpp
- Committer:
- piroro4560
- Date:
- 2019-06-11
- Revision:
- 3:4fe8e6e455d0
- Parent:
- 2:8d16db0d55b7
File content as of revision 3:4fe8e6e455d0:
#include "mbed.h" //wheel_test4 #include "controller.h" #include "ikarashiMDC.h" #include "R1370.h" #include "omni_wheel.h" #include "PID.h" #define PI 3.141593 PID pid1(5.0,0.001,0.0001,0.01); OmniWheel omni(4); Controller pad(PC_10, PC_11, 208); R1370 R1370(PB_6,PA_10); Serial pc(USBTX, USBRX, 115200); Serial serial(PC_6, PC_7, 115200); DigitalOut serialcontrol(D10); //DigitalOut reset(PC_9); ikarashiMDC motor[]= { ikarashiMDC(&serialcontrol,2,0,SM,&serial), ikarashiMDC(&serialcontrol,2,1,SM,&serial), ikarashiMDC(&serialcontrol,2,2,SM,&serial), ikarashiMDC(&serialcontrol,2,3,SM,&serial), }; int main() { // reset = 1; motor[0].braking = true; int b[11], b2[11]; double rad[2], dis[2], value[3], X, Y, original_angle, now_angle, start_angle, spin_power; /* * original_angle : * now_angle : * start_angle : */ pid1.setInputLimits(-180,180); pid1.setOutputLimits(-0.3,0.3); pid1.setBias(0); pid1.setMode(1); pid1.setSetPoint(0);//// omni.wheel[0].setRadian(PI * 1.0 / 4.0); omni.wheel[1].setRadian(PI * 3.0 / 4.0); omni.wheel[2].setRadian(PI * 5.0 / 4.0); omni.wheel[3].setRadian(PI * 7.0 / 4.0); while(1){ if(pad.receiveState()){ for(int i = 0; i < 13; i++){ b[i] = 1 - pad.getButton1(i); } for(int i = 0; i < 2; i++){ rad[i] = pad.getRadian(i); dis[i] = pad.getNorm(i); rad[i] = PI - rad[i]; } X = dis[0] * cos(rad[0]); Y = dis[0] * sin(rad[0]); R1370.update(); /*ここまでテンプル*/ start_angle = R1370.getAngle(); /* 旋回 */ /* リセット */ if(b[6] == 1){ original_angle=start_angle; } now_angle = start_angle - original_angle; pid1.setProcessValue(now_angle); spin_power = pid1.compute(); omni.computeXY(X,Y,-spin_power); pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||\n\r",X, Y, start_angle, now_angle, spin_power); for(int i = 0; i < 4; i++)motor[i].setSpeed(omni.wheel[i]); for(int i = 0; i < 11; ++i)b2[i] = b[i]; }else{ pc.printf("error\n\r"); for (int i = 0; i < 4; i++)motor[i].setSpeed(0); } } }