Ryu Anzawa
wheel_test5 ver_1
Dependencies: QEI omni_wheel PID R1370 Controller ikarashiMDC
Revision 0:424b608bab8c, committed 2019-06-20
- Comitter:
- piroro4560
- Date:
- Thu Jun 20 07:03:57 2019 +0000
- Commit message:
- ver_1
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Controller.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/Controller/#875532e626ee
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/QEI/#fe23b32e62ca
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/R1370.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/tknara/code/R1370/#96f91d9e3bff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ikarashiMDC.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/teams/NHK-Robocon2016_Nagaoka_B_Team/code/ikarashiMDC/#ea34af94e90c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Jun 20 07:03:57 2019 +0000
@@ -0,0 +1,109 @@
+#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);
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#a8e5a4cb0f4facb615c32306d9b509aec07a0b5a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omni_wheel.lib Thu Jun 20 07:03:57 2019 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/teams/NHK-Robocon2016_Nagaoka_B_Team/code/omni_wheel/#cfec945ea421