Revision 0:13ab960fc61f, committed 2013-03-08

Comitter:

akudohune

Date:

Fri Mar 08 07:13:29 2013 +0000

Commit message:

ver1_2_0;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC6352.lib	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/HMC6352/#83c0cb554099

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.cpp	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,205 @@
+
+#include "mbed.h"
+#include "IR.h"
+
+
+extern Timer timer_ir;
+extern Serial pc; // tx, rx 
+
+int direction = 0;
+int Distance  = 0;
+
+int IR_found;
+
+
+void IR_Position(){
+
+    int ir_value[ALL_IR+100] = {0};
+
+    int active_ir = 0;     /* 今回更新する赤外線の番号 */
+    int memory_ir = 0;         /*赤外線時間カウンタ*/
+    int flag_ir = 0;
+    int value = 0;
+
+    for(int i=0; i<ALL_IR; i++) {
+        flag_ir = 1;
+
+        DigitalIn sensor_ir(ir_num[active_ir]); /* 今回更新する赤外線の個体を呼び出す */
+
+        timer_ir.start();   /* タイマー起動 */
+
+        if(sensor_ir) {              /* もし立ち上がっていたら */
+            while(sensor_ir) {       /* 立ち下がるまで待つ */
+                if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+                    flag_ir = 0;
+                    break;  /* 立ち上がっている時間が指定時間越えたらブレイク */
+                }
+            }
+        }
+
+        timer_ir.stop();    /* タイマー停止 */
+        timer_ir.reset();   /* タイマーリセット */
+
+        if(flag_ir) {
+            timer_ir.start();   /* タイマー起動 */
+
+            while(!(sensor_ir)) {       /* 立ち上がるまで待つ */
+                if(timer_ir.read_us() >= IR_TIME_NOTFOUND) {
+                    flag_ir = 0;
+                    break;  /* 立ち上がっている時間が指定時間越えたらブレイク */
+                }
+            }
+        }
+
+        /*ボールが指定時間内に見つかっていたら*/
+        if(flag_ir) {
+            memory_ir = timer_ir.read_us();
+
+            while(1) {
+                if((timer_ir.read_us()-memory_ir)>=IR_TIME_NOTFOUND)break;
+
+                if(!(sensor_ir)) {
+                    //value = moving_ave( (timer_ir.read_us()-memory_ir)/10 , active_ir );
+                    value = (timer_ir.read_us()-memory_ir)/10;
+
+                    break;
+                }
+            }
+        } else {
+            /*ボールが見つかっていない場合*/
+            value = 0;
+        }
+        timer_ir.stop();    /* タイマー停止 */
+        timer_ir.reset();   /* タイマーリセット */
+
+        memory_ir = 0;
+
+        ir_value[active_ir] =  value; //direction array
+
+        active_ir++;
+
+        if( active_ir >= ALL_IR) {
+            active_ir = 0;
+
+            /***********direction***********/
+    
+            int min = 100,youso_min = 100;
+    
+            for(int i = 0; i<DIREC_IR; i++) {
+                if((ir_value[i]<min)&&(ir_value[i])) {
+                    min = ir_value[i];
+                    youso_min = i;
+                }
+            }
+    
+            double hiritu = 0;
+    
+            int direc = 0;
+    
+            if(youso_min == 0) {
+                hiritu = (double)ir_value[7]/(double)ir_value[1];
+            } else if(youso_min ==7) {
+                hiritu = (double)ir_value[6]/(double)ir_value[0];
+            } else {
+                hiritu = (double)ir_value[youso_min-1]/(double)ir_value[youso_min+1];
+            }
+            
+            if((hiritu <= 0.85)&&(youso_min != 0)) {
+                direc = youso_min*2-1;
+            }else if((hiritu <= 0.85)&&(youso_min == 0)){
+                direc = 15; 
+            }else if(hiritu >= 1.15) {
+                direc = youso_min*2+1;
+            } else {
+                direc = youso_min*2;
+            }
+            
+            /*if(youso_min == 0){
+                direc = 0;   
+            }*/
+    
+            /*******  direction end  *******/
+    
+            /*******     distance    *******/
+    
+            int dista;
+            
+            if((ir_value[youso_min]<=28 + TERM)) {
+                dista = 30;
+            } else if((ir_value[youso_min]>28 + TERM)&&(ir_value[youso_min]<=35 + TERM)) {
+                dista = 90;
+            } else if((ir_value[youso_min]>35 + TERM)&&(ir_value[youso_min]<=40 + TERM)) {
+                dista = 120;
+            } else if( ir_value[youso_min]>40 + TERM) {
+                dista = 180;
+            } else {
+                dista = 0;
+            }
+            
+            int count_ir = 0,total_ir = 0;
+            
+            for(int i=0; i<DIREC_IR; i++){
+                if(ir_value[i]){
+                    total_ir += ir_value[i];
+                    count_ir++;
+                }
+            }
+            
+            double hihhihi = 0;
+            
+            hihhihi = (double)ir_value[youso_min]/(double)ir_value[8];
+            
+            
+            if((direc == 0)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 1)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 2)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 3)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 4)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 5)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 6)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 7)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 8)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 9)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 10)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 11)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 12)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            }else if((direc == 13)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 14)&&(hihhihi  >= 0.65)){
+                dista = 10;
+            }else if((direc == 15)&&(hihhihi  >= 0.80)){
+                dista = 10;
+            } 
+            
+            int count = 0;
+            
+            for(int i=0;i<DIREC_IR;i++){
+                if(ir_value[i])count++;
+            } 
+            
+            if(count)   IR_found = 1;
+            else        IR_found = 0;
+            
+
+            /********  distance end  *******/
+            
+            direction = direc;
+            Distance  = dista;
+            
+            //printf("derection:%d distance:%d\n",direction,Distance);
+        }
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IR.h	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,63 @@
+
+
+#define IR_TIME_NOTFOUND 833    /* 見つけられなかったと判断するまでの時間(単位：us) */
+#define IR_COUNTMAX 487     /*最大パルス幅 パルスの存在しうる最大時間は487us*/
+#define ALL_IR 9
+#define DIREC_IR 8
+#define DIRECTION 16
+#define TERM 0
+#define DELTA -2
+#define SWAP(type,a,b) { type temp = a; a = b; b = temp; }
+
+
+/* 赤外線センサに使うpinを配列に格納 */
+PinName ir_num[ALL_IR] = {
+    p13,
+    p14,
+    p15,
+    p16,
+    p17,
+    p18,
+    p19,
+    p20,
+    p30
+};
+
+int Convert_Direction[DIRECTION] = {
+    90,
+    67,
+    45,
+    22,
+    0,
+    337,
+    315,
+    292,
+    270,
+    247,
+    225,
+    202,
+    180,
+    157,
+    135,
+    112
+};
+
+//ball direction
+double ball_sankaku[16][2] = {
+    {0     , 1    },
+    {0.390 , 0.920},
+    {0.707 , 0.707},
+    {0.927 , 0.374},
+    {1     , 0    },
+    {0.920 ,-0.390},
+    {0.707 ,-0.707},
+    {0.374 ,-0.927},
+    {0     ,-1    },
+    {-0.390,-0.920},
+    {-0.707,-0.707},
+    {-0.927,-0.374},
+    {-1    , 0    },
+    {-0.920, 0.390},
+    {-0.707, 0.707},
+    {-0.374, 0.927}
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,172 @@
+#include <math.h>
+#include <sstream>
+#include "mbed.h"
+#include "HMC6352.h"
+#include "PID.h"
+#include "main.h"
+
+
+/***********  Serial interrupt  ***********/
+
+void Tx_interrupt()
+{
+    array(speed[0],speed[1],speed[2],speed[3]);
+    driver.printf("%s",StringFIN.c_str());
+    //pc.printf("%s",StringFIN.c_str());
+    //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+}
+/*
+void Rx_interrupt()
+{
+    if(driver.readable()){
+        //pc.printf("%d\n",driver.getc());
+    }
+}*/
+
+
+/***********  Serial interrupt end **********/
+
+void PidUpdata()
+{
+    float deviation = 0;
+    
+    
+    if(standard < 180.0){
+        if((compass.sample() / 10.0) < standard){
+            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+        }else if((compass.sample() / 10.0) < 180.0 + standard){
+            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+        }else{
+            inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard);
+        }
+    }else if(standard > 180.0){
+        if((compass.sample() / 10.0) > standard){
+            inputPID = 180.0 +((compass.sample() / 10.0) - standard);
+        }else if((compass.sample() / 10.0) > standard - 180.0){
+            inputPID = 180.0 -(standard - (compass.sample() / 10.0));
+        }else{
+            inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard));
+        }
+    }else{
+        inputPID = compass.sample() / 10.0;
+    }
+    
+    if(inputPID < 0){
+        inputPID *= -1; //
+    }
+        
+    
+    //pid.setProcessValue(inputPID);
+    
+    deviation = (inputPID - 180.0);
+    
+    compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0));
+    
+    if(compassPID > 100)compassPID = 100;
+    else if(compassPID < -100)compassPID = -100;
+    
+    //pc.printf("%f\n",compassPID);
+    
+    lastData = deviation;
+}
+
+
+
+void move(int vx, int vy, int vs)
+{
+    double motVal[MOT_NUM] = {0};
+    
+    motVal[0] = (double)((0.5 * vx)  + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
+    motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
+    motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs));
+    motVal[3] = (double)((0.5  * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs));
+    
+    for(uint8_t i = 0;i < MOT_NUM;i++){
+        if(motVal[i] > 100)motVal[i] = 100;
+        else if(motVal[i] < -100)motVal[i] = -100;
+        speed[i] = motVal[i];
+    }
+    /*
+    pc.printf("speed1 = %d\n",speed[0]);
+    pc.printf("speed2 = %d\n",speed[1]);
+    pc.printf("speed3 = %d\n",speed[2]);
+    pc.printf("speed4 = %d\n\n",speed[3]);      
+    */
+    ////pc.printf("%s",StringFIN.c_str());
+}
+
+void init()
+{
+    
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    StartButton.mode(PullUp);
+    CalibEnterButton.mode(PullUp);
+    CalibExitButton.mode(PullUp);
+    driver.baud(BAUD_RATE);
+    wait_ms(MOTDRIVER_WAIT);
+    driver.printf("1F0002F0003F0004F000\r\n"); 
+    
+    led1 = ON;
+    
+    while(StartButton){
+        if(!CalibEnterButton){
+            led1 = OFF;
+            led2 = ON;
+            compass.setCalibrationMode(ENTER);
+            while(CalibExitButton);
+            compass.setCalibrationMode(EXIT);
+            led2 = OFF;
+            led3 = ON;
+         }
+    }
+    
+    standard = compass.sample() / 10.0;
+    led1 = OFF;
+    led3 = OFF;
+    
+    pid.setInputLimits(0.0, 360.0);
+    pid.setOutputLimits(-50.0, 50.0);
+    pid.setBias(0.0);
+    pid.setMode(AUTO_MODE);
+    pid.setSetPoint(180.0);
+    
+    pidUpdata.attach(&PidUpdata, 0.06);
+    //ultrasonic.attach(&Ultrasonic, 0.1);
+    IR.attach(&IR_Position,0.04);
+    driver.attach(&Tx_interrupt, Serial::TxIrq);
+    //driver.attach(&Rx_interrupt, Serial::RxIrq);
+    
+    timer2.start();
+}
+
+int main()
+{
+    int vx=0,vy=0,vs=0;
+    
+    init();
+           
+    while(1) {
+        vs = compassPID;
+        
+        if(IR_found){
+            if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){
+                vx = (int)(20*ball_sankaku[direction][0]);
+                vy = (int)(20*ball_sankaku[direction][1]);
+            }else{
+                vx = (int)(10*ball_sankaku[direction][0]);
+                vy = (int)(10*ball_sankaku[direction][1]);
+            }
+            
+            if(Distance <= 10){
+                vx *= -1;
+                vy *= -1;
+            }
+        }else{
+            vx = 0;
+            vy = 0;
+        }
+        
+        
+        move(vx,vy,vs);
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.h	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,48 @@
+
+
+#define RATE    0.004173
+#define Long    1.0
+#define ENTER   0
+#define EXIT    1
+#define MOT_NUM 4
+#define MOTDRIVER_WAIT  300 //ms
+#define BAUD_RATE       115200
+#define ON      1
+#define OFF     0
+
+DigitalOut led1(LED1); 
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+HMC6352 compass(p9, p10);
+Serial driver(p28, p27); // tx, rx 
+Serial pc(USBTX, USBRX); // tx, rx 
+DigitalIn StartButton(p21);
+DigitalIn CalibEnterButton(p22);
+DigitalIn CalibExitButton(p23);
+PID pid(1.5, 0.0, 0.000002, RATE);
+Ticker pidUpdata;
+Ticker IR;
+Ticker ultrasonic;
+Timer timer1;
+Timer timer2;
+Timer timer_ir;     /* 赤外線用タイマー */
+
+
+int speed[MOT_NUM] = {0};
+
+static float lastData = 0.0;
+static float inputPID = 180.0;
+static float standard;
+static float compassPID = 0.0;
+
+extern string StringFIN;
+
+extern int direction;
+extern int Distance;
+extern int IR_found;
+extern double ball_sankaku[16][2];
+
+extern void Ultrasonic(void);
+extern void IR_Position(void);
+extern void PidUpdata(void);
+extern void array(int,int,int,int);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5e5da4a5990b
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.cpp	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,45 @@
+
+#include "mbed.h"
+#include "ultrasonic.h"
+
+
+extern Timer timer2;
+extern Serial pc; // tx, rx 
+
+uint16_t ultrasonicVal[ALL_ULTRASONIC] = {0};
+
+
+void Ultrasonic()
+{
+    for(uint8_t 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() > 1000){   //1ms以上応答なし
+                ultrasonicVal[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        } 
+        timer2.reset();
+        while(PingPin == 1){
+            if((timer2.read_us() > 18500) || (flag == 1)){  //18.5ms以上のパルス
+                ultrasonicVal[i] =  PING_ERR;
+                flag = 1;
+                break;
+            }
+        }
+        if(flag == 0){
+            ultrasonicVal[i] = timer2.read_us();
+        }
+        //pc.printf("compass.sample = %f\n",compass.sample() / 1.0);
+    }
+    //pc.printf("%d\n",ultrasonicVal[2]);
+    
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ultrasonic.h	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,12 @@
+
+
+#define ALL_ULTRASONIC  3
+#define PING_ERR    0xFFFF
+
+
+PinName ultrasonic_pin[ALL_ULTRASONIC] = {
+    p25,
+    p26,
+    p29,
+    //p30,
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/wordString.cpp	Fri Mar 08 07:13:29 2013 +0000
@@ -0,0 +1,70 @@
+
+#include <sstream>
+#include "mbed.h"
+
+string StringFIN;
+
+using namespace std;
+
+
+extern Serial pc; // tx, rx 
+
+string IntToString(int number)
+{
+  stringstream ss;
+  ss << number;
+  return ss.str();
+}
+
+void array(int power1,int power2,int power3,int power4)
+{
+    int input[4] = {power1,power2,power3,power4};
+    int value = 0;
+    string StringA[4] = {"0","0","0","0"};
+    
+    
+    string StringX = "0";
+    string StringY = "0";
+    string StringZ = "0";
+    string String0 = "0";
+    
+    StringFIN = "0";
+    
+    for(uint8_t i = 0 ; i < 4; i++){
+        
+        value = input[i];
+        
+        StringX =  IntToString(i+1);
+        
+        if( (value < 0) && (value >= -100) ){
+            StringY = "R";
+            value = abs(value);
+            StringZ = IntToString(value);
+        }else if( (value >= 0) && (value <= 100) ){
+            StringY = "F";
+            StringZ = IntToString(value);
+        }else{
+            StringA[i] = StringX + "F" + "000";
+            
+        }
+        
+        if(value < 10){
+            String0 = "00";
+            StringZ = String0 + StringZ;
+        }else if(value < 100)
+        {
+            String0 = "0";
+            StringZ = String0 + StringZ;
+        }else{
+            
+        }
+        
+        StringA[i] = (StringX + StringY + StringZ);
+        
+        if(i == 0)StringFIN  = StringA[i];
+        else StringFIN  += StringA[i];
+        
+    }
+    
+    StringFIN += "\r\n";   
+}
\ No newline at end of file