Diff: main.cpp

Revision:

0:4644bf6bca6a

Child:

1:f465d89a26b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,96 @@
+#include "mbed.h"
+//#include "QEI.h"
+#include "HMC6352.h"
+#include "Servo.h"
+#include "main.h"
+
+#define OUTRANGE_MAX    0.2//pid
+//#define ROTATE_PER_REVOLUTIONS  360//enko-da no bunkainou
+
+//QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
+//HMC6352 compass(p28/*sda*/, p27/*scl*/);
+Servo myservo1(p21);
+Servo myservo2(p22);
+DigitalOut myled(LED1);
+Ticker interrupt;
+
+double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD);
+
+double proportional = 0;
+uint16_t com_val = 0;
+
+
+void tic_sensor()
+{
+    //int temp = (double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4);
+    Ultrasonic();
+    /*com_val = (compass.sample() / 10 + 180)%360;//180°を中間値にする
+    //proportional = PIDRead(com_val, 180, 0.002, 0, 0);
+    
+    proportional = 0.002*(com_val-180);
+    
+    if(proportional > OUTRANGE_MAX){
+        proportional = OUTRANGE_MAX;
+    }else if(proportional < -OUTRANGE_MAX){
+        proportional = -OUTRANGE_MAX;
+    }*/
+}
+
+
+/*double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD)
+{
+    static double diff[2] = {0};
+    double p,i,d = 0;
+    double m;
+    static int16_t integral = 0;
+    
+    diff[0] = diff[1];
+    diff[1] = (double)(sensor - target);
+    integral += (diff[0] + diff[1])/2;
+    
+    p = KP * diff[1];
+    i = KI * integral;
+    d = KD * (diff[1] - diff[0]);
+    m = p+i+d;
+      
+    if(m > OUTRANGE_MAX){
+        m = OUTRANGE_MAX;
+    }else if(m < -OUTRANGE_MAX){
+        m = -OUTRANGE_MAX;
+    }
+    
+    return m;
+}*/
+
+
+
+int main() {
+    
+    
+    
+    
+    //printf("test\n");
+    
+    timer2.start();
+    interrupt.attach(&tic_sensor, 0.1/*sec*/);
+    //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    //printf("test%d\n",com_val);
+    
+    while(1) {
+    wait(0.1);
+        
+        
+        printf("pid:%.5d\n", ultrasonicVal[0]);
+        
+        if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){
+            myservo1 = 0.5;
+            myservo2 = 0.5;
+        }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){
+            myservo1 = 0.7 + proportional;
+            myservo2 = 0.7;
+        }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){
+            myservo1 = 0.3;
+            myservo2 = 0.3 + proportional;
+        }
+    }
+}