aa
Dependencies: wheel_an QEI omni_wheel PID R1370 Controller ikarashiMDC
Revision 1:5c0b7355adcd, committed 2019-05-16
- Comitter:
- skouki
- Date:
- Thu May 16 08:43:30 2019 +0000
- Parent:
- 0:46c70e5b719d
- Commit message:
- a
Changed in this revision
diff -r 46c70e5b719d -r 5c0b7355adcd PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Thu May 16 08:43:30 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
diff -r 46c70e5b719d -r 5c0b7355adcd QEI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu May 16 08:43:30 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca
diff -r 46c70e5b719d -r 5c0b7355adcd main.cpp
--- a/main.cpp Thu May 16 07:41:44 2019 +0000
+++ b/main.cpp Thu May 16 08:43:30 2019 +0000
@@ -2,35 +2,41 @@
#include "controller.h"
#include "ikarashiMDC.h"
#include "R1370.h"
-#include "wheel_an.h"
#define PI 3.141593
+#include"omni_wheel.h"
+#include"PID.h"
+PID pid1(3.0,3.0,0.000005,0.01);
+OmniWheel omni(3);
Controller pad(PC_10, PC_11, 208);
-R1370 R1370(D10,D2);
+R1370 R1370(PB_6,PA_10);
Serial pc(USBTX, USBRX, 115200);
Serial serial(PC_6, PC_7, 115200);
-DigitalOut serialcontrol(PD_2);
+DigitalOut serialcontrol(D10);
-ikarashiMDC ikarashi[]
+ikarashiMDC motor[]=
{
ikarashiMDC(&serialcontrol,2,0,SM,&serial),
ikarashiMDC(&serialcontrol,2,1,SM,&serial),
ikarashiMDC(&serialcontrol,2,2,SM,&serial)
};
-Wheelan wheel[]
-{
- Wheelan(PI * 1 / 6),
- Wheelan(PI * 5 / 6),
- Wheelan(PI * 9 / 6)
-};
-
int main()
{
- ikarashi[0].braking = true;
+ pid1.setInputLimits(-90,90);
+ pid1.setOutputLimits(-0.5,0.5);
+ pid1.setBias(0);
+ pid1.setMode(1);
+ pid1.setSetPoint(0);
+ omni.wheel[0].setRadian(PI * 1.0 / 6.0);
+ omni.wheel[1].setRadian(PI * 5.0 / 6.0);
+ omni.wheel[2].setRadian(PI * 9.0 / 6.0);
+
+ motor[0].braking = true;
int b[11];
double rad[2],dis[2];
double value[3];
+ pc.printf("saaa");
while(1){
if(pad.receiveState()){
for(int i = 0; i < 13; i++){
@@ -41,36 +47,22 @@
dis[i] = pad.getNorm(i);
rad[i] = PI - rad[i];
}
+ double X = dis[0]*cos(rad[0]);
+ double Y = dis[0]*sin(rad[0]);
R1370.update();
- pc.printf("degree : %.3lf ", R1370.getAngle());
-
- for (int i = 0; i < 3; ++i)
- {
- value[i] = wheel[i].an_uc(rad[1], dis[1]);
- }
-
- if(b[7] == 1 && b[9] == 0) {
- value[0] -= 0.3;
- value[1] -= 0.3;
- value[2] -= 0.3;
- } else if (b[9] == 1 && b[7] == 0) {
- value[0] += 0.3;
- value[1] += 0.3;
- value[2] += 0.3;
- }
-
- for (int i = 0; i < 3; ++i)
- {
- pc.printf("%lf ", value[i]);
- ikarashi[i].setSpeed(value[i]);
- }
- pc.printf("\n\r");
-
+ double angle = R1370.getAngle();
+ pid1.setProcessValue(angle);
+ double spin_power = pid1.compute();
+ omni.computeXY(X,Y,-spin_power);
+ pc.printf("%.3f||%.3f||%.3f||%.3f\n\r",X,Y,angle,spin_power);
+ for(int i = 0; i < 3; i++)
+ motor[i].setSpeed(omni.wheel[i]);
+
}else{
pc.printf("error\n\r");
for (int i = 0; i < 3; i++)
{
- ikarashi[i].setSpeed(0);
+ motor[i].setSpeed(0);
}
}
}
diff -r 46c70e5b719d -r 5c0b7355adcd mbed-os.lib --- a/mbed-os.lib Thu May 16 07:41:44 2019 +0000 +++ b/mbed-os.lib Thu May 16 08:43:30 2019 +0000 @@ -1,1 +1,1 @@ -https://github.com/ARMmbed/mbed-os/#c9e63f14085f5751ff5ead79a7c0382d50a813a2 +https://github.com/ARMmbed/mbed-os/#6f338f8915d22737ecb5f3de508584b6fb00749e
diff -r 46c70e5b719d -r 5c0b7355adcd omni_wheel.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omni_wheel.lib Thu May 16 08:43:30 2019 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421