robocup

Dependencies:   HMC6352 PID mbed

Files at this revision

API Documentation at this revision

Comitter:
akudohune
Date:
Fri Mar 08 07:13:29 2013 +0000
Commit message:
ver1_2_0;

Changed in this revision

HMC6352.lib Show annotated file Show diff for this revision Revisions of this file
IR.cpp Show annotated file Show diff for this revision Revisions of this file
IR.h Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
ultrasonic.cpp Show annotated file Show diff for this revision Revisions of this file
ultrasonic.h Show annotated file Show diff for this revision Revisions of this file
wordString.cpp Show annotated file Show diff for this revision Revisions of this file
--- /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