Revision 0:4644bf6bca6a, committed 2013-07-31

Comitter:

OGA

Date:

Wed Jul 31 04:43:58 2013 +0000

Child:

1:f465d89a26b0

Commit message:

????

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/simon/code/Servo/#36b69a7ced07

--- /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;
+        }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,7 @@
+#include "mbed.h"
+
+Timer timer2;
+
+extern double ultrasonicValue[4];
+extern uint16_t ultrasonicVal[4];
+extern void Ultrasonic(void);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.cpp	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "ping.h"
+ 
+//DigitalOut myled = LED1; 
+ 
+extern Timer timer2;
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC];
+double ultrasonicValue[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(int i = 0 ; i < ALL_ULTRASONIC; i++){
+       
+        uint8_t flag = 0;
+    
+        DigitalOut PingPinOut(ultrasonic_pin[i]);
+        PingPinOut = 1;
+        wait_us(10);
+        PingPinOut = 0;
+        DigitalIn PingPin(ultrasonic_pin[i]);
+        timer2.reset();
+        while(PingPin == 0){
+            if(timer2.read_us() > 1500){   //1.5ms以上応答なし
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+  
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicValue[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+      
+        if(flag == 0){
+            ultrasonicValue[i] = timer2.read_us() / 1000000.0 / 2.0 * 340.0 * 1000.0; //mm  MAX:3145
+            ultrasonicVal[i] = (int)(ultrasonicValue[i] * 10.0);
+        }else{
+            ultrasonicVal[i] = PING_ERR;
+        }
+
+    }
+} 
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ping/ping.h	Wed Jul 31 04:43:58 2013 +0000
@@ -0,0 +1,10 @@
+#define ALL_ULTRASONIC  4
+#define PING_ERR        0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p17,
+    p18,
+    p19,
+    p20,
+};