Diff: main.cpp

Revision:

3:2fb6302e0cda

Parent:

2:dcd12798a336

--- a/main.cpp	Fri Sep 11 09:38:43 2015 +0000
+++ b/main.cpp	Mon Sep 14 07:58:08 2015 +0000
@@ -2,7 +2,7 @@
 #include "QEI.h"
 #define DELTA_T 0.004   //処理周期4msec
 #define KP 0.0          //比例定数
-#define KI 0.0          //積分定数
+#define KI 0.0f          //積分定数
 #define KD 0.0          //微分定数
 #define n  1          //最大値最小値設定
 
@@ -15,6 +15,7 @@
 PwmOut mypwm030(D3);    //アームのモータ？
 PwmOut mypwm031(D4);
 
+#include "main_new.h"
 #define ROTATE_PER_REVOLUTIONS  24
 QEI wheel01(   D7,D8,     //A相のピン、B相のピン
              NC,                             //回転数数えるピン（任意）
@@ -34,15 +35,15 @@
 
 F32 pid_sample(int a,int b)
 {
-    int turn;
+    float turn;
     F32 p,i,d;
-    
+    i += 0.0f;
     diff[0] = diff[1];
     diff[1]= a - b;
-    integral += (diff[1]+diff[0])/2.0*DELTA_T;
+    integral += (diff[1]+diff[0])/2.0*DELTA_T+ 0.0f;
     
     p = KP*diff[1];
-    i = KI*integral;
+    i = KI * integral;
     d = KD *(diff[1]-diff[0])/DELTA_T;
     
     turn = p + i + d;
@@ -62,7 +63,7 @@
     
     mypwm021 = 1.0;
     mypwm022 = 0.0;
-    
+    Motor01(0.8);
 /*    mypwm.period_ms(10);
     mypwm.pulsewidth_ms(1);  */
  for(int i = s; i < wheel01.getPulses();)
@@ -71,8 +72,8 @@
                 else
                 {mypwm022 = pid_sample(wheel02.getPulses(),wheel01.getPulses());}
                 
-                if(mypwm012<=0.1&&mypwm022<=0.1)
-                    {mypwm012=mypwm012-0.1;mypwm022=mypwm022-0.1;}
+                if(mypwm012<=0.1f&&mypwm022<=0.1f)
+                    {mypwm012=mypwm012-0.1f;mypwm022=mypwm022-0.1f;}
                                                             }
     mypwm030 = 1.0;
     mypwm031 = 1.0;
@@ -95,8 +96,8 @@
                 else
                      {mypwm021 = pid_sample(wheel02.getPulses(),wheel01.getPulses());}
                      
-                    if(mypwm011>=0.1&&mypwm021>=0.1)
-                        {mypwm011=mypwm011-0.1;mypwm022=mypwm022-0.1;}
+                    if(mypwm011>=0.1f&&mypwm021>=0.1f)
+                        {mypwm011=mypwm011-0.1f;mypwm022=mypwm022-0.1f;}
                                                             }
             
     myled = 1;   //パルスがmの値にいくまで