![](/media/cache/profiles/sirooni.jpg.50x50_q85.jpg)
wheel_test5 ver_1
Dependencies: QEI omni_wheel PID R1370 Controller ikarashiMDC
main.cpp
- Committer:
- piroro4560
- Date:
- 2019-06-20
- Revision:
- 0:424b608bab8c
File content as of revision 0:424b608bab8c:
#include "mbed.h" //wheel_test5 #include "controller.h" #include "ikarashiMDC.h" #include "R1370.h" #include "omni_wheel.h" #include "PID.h" #define PI 3.1415926535897932384626 PID pid1(5.0,0.2,0.000095,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), }; double sign(double a){ return(a>0)-(a<0); } int main() { // reset = 1; motor[0].braking = true; int b[11], b2[11], b3[11], angle_state; double rad[2], dis[2], value[3], X, Y, original_angle=0, now_angle, start_angle, zero=0, spin_power; double deviation = 0.08; /** * now_angle : 今の角度 * start_angle : 素の角度 * original_angle : 0,-90,90,180 * deviation : 差 * angle_state : 90度毎 * zero : 零点合わせ */ 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(); for (int i = 0; i < 11; i++) b3[i] = b2[i] - b[i]; /** * ここまでテンプル */ start_angle = R1370.getAngle(); if ( b3[9] == 1) angle_state++; if ( b3[7] == 1) angle_state--; angle_state %= 4; switch( ( angle_state + 8 ) % 4 ){ case 1: original_angle=180; break; case 2: original_angle=-90; break; case 3: original_angle=0; break; default : original_angle=90; break; } if ( b[6] ) original_angle = start_angle; now_angle = start_angle - original_angle; if ( now_angle > 180 ) now_angle = now_angle - 360; if ( now_angle < -180 ) now_angle = now_angle + 360; pid1.setProcessValue(now_angle); /** * 定型文 */ spin_power = pid1.compute(); omni.computeXY(X,Y,-spin_power); pc.printf("%.3f||%.3f||%.3f||%.3f||%.3f||%d\n\r",X, Y, start_angle, now_angle, spin_power, angle_state); 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); } } }