ハセオムニのプログラム
Dependencies: vnh5019 SerialMultiByte omni_wheel PID jy901 MotorSMLAP PS3
Diff: main.cpp
- Revision:
- 12:8d5e9965e8f2
- Parent:
- 11:85568f4e4eaf
- Child:
- 13:0810457baab8
diff -r 85568f4e4eaf -r 8d5e9965e8f2 main.cpp --- a/main.cpp Fri Jan 24 07:32:57 2020 +0000 +++ b/main.cpp Fri Jan 24 08:58:19 2020 +0000 @@ -3,14 +3,9 @@ #include "math.h" #include "omni_wheel.h" #include "motorsmlap.h" -#include "jy901.h" #include "SerialMultiByte.h" -#include "PID.h" -JY901 jy(PB_9, PB_8); //sda, scl - -PID angle(2.0,0,0,0.01); -Serial pc(USBTX, USBRX, 9600); +Serial pc(USBTX, USBRX, 115200); PwmOut beep(PA_0); DigitalOut led1(PA_11); @@ -68,13 +63,7 @@ int main() { setup(); - float x, y,now_angle; - jy.calibrateAll(50); - angle.setInputLimits(-180.0,180.0); - angle.setOutputLimits(-1,1); - angle.setBias(0); - angle.setMode(1); - angle.setSetPoint(0); + float x, y; while (true) { led2=1; int c = pc.getc(); @@ -94,23 +83,17 @@ x=0; y=0; } - now_angle = jy.getZaxisAngle(); - - if(now_angle>180 && now_angle<360){ - now_angle = now_angle-360; - } - angle.setProcessValue(now_angle); omni.computeXY( x, y, 0, 0, - angle.compute() + 0 ); for(int i=0; i<3; i++){ motor[i].setMotorSpeed(omni.wheel[i].getOutput()*0.8); } - pc.printf("X = <%f>,Y = <%f>,angle= <%f>\r\n",x,y,now_angle); + pc.printf("X = <%f>,Y = <%f>\r\n",x,y); } } \ No newline at end of file