Revision 2:054444aa1990, committed 2014-12-23

Comitter:

ryuna

Date:

Tue Dec 23 08:08:24 2014 +0000

Parent:

1:7d4921b5d638

Commit message:

???????; SetUp; StartLoop; IrReceive; LineReceive?

Changed in this revision

diff -r 7d4921b5d638 -r 054444aa1990 L6470_lib.lib
--- a/L6470_lib.lib	Tue Dec 16 09:13:08 2014 +0000
+++ b/L6470_lib.lib	Tue Dec 23 08:08:24 2014 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/teams/2014/code/L6470_lib/#f2854fe95fdd
+http://developer.mbed.org/teams/2014/code/L6470_lib/#e89f842fbeda

diff -r 7d4921b5d638 -r 054444aa1990 main.cpp
--- a/main.cpp	Tue Dec 16 09:13:08 2014 +0000
+++ b/main.cpp	Tue Dec 23 08:08:24 2014 +0000
@@ -35,6 +35,18 @@
  *
  ******************************/
 
+
+/*
+
+****以下IRはNighは距離が近いものとする．
+
+*/
+
+
+
+
+
+
 #include "mbed.h"
 #include "L6470.h"
 #include "PID.h"
@@ -42,6 +54,11 @@
 #include <math.h>
 #include <sstream>
 
+#define ADDRESS_R 0xA0
+#define ADDRESS_L 0xC0
+
+#define READ_IR 0x01
+
 DigitalIn   DebugSw[4] = {p22,p23,p24,p25};
 DigitalIn   StartSw(p26,PullUp);
 DigitalOut  Led[4] = {LED1,LED2,LED3,LED4};
@@ -60,13 +77,77 @@
 int speed[4] = {0};            
 
 
-void move(int vx, int vy, int vs, int vk){
+unsigned int IrReceive(uint8_t *far){
+    /*
+    Slave側はIRを要求した場合IRのみを返してくるとする．
+    irは値が大きいほうが近いと仮定する
+    
+    
+    */
+    char data_r[4],data_l[4];
+    bool val;
+    Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか
+    
+    val = Sensor.read(ADDRESS_R|1, data_r, 4);// PINGデータを受信
+    Led[0] = val;
+    wait_ms(5);
+    val = Sensor.read(ADDRESS_L|1, data_l, 4);
+    Led[0] = !val;
+    
+    if((data_r[3]-data_l[3])>0)
+        *far = data_r[0]%36%6*6 + (data_l[0]%36%6+6);
+        
+    if(data_r[2]>data_l[2]){
+        if(data_r[2]>data_l[1]){   
+            return data_r[0]/36*12*12*12 + data_r[0]%36/6*12*12 + (data_l[0]/36+6)*12 + (data_l[0]%36/6+6);
+        }else{
+            return data_r[0]/36*12*12*12 +(data_l[0]/36+6)*12*12+ data_r[0]%36/6*12 +(data_l[0]%36/6+6);
+        }
+    }else{
+        if(data_l[2]>data_r[1]){   
+            return (data_l[0]/36+6)*12*12*12 + (data_l[0]%36/6+6)*12*12 + data_r[0]/36*12 + data_r[0]%36/6;
+        }else{
+            return (data_l[0]/36+6)*12*12*12 +data_r[0]/36*12*12+ (data_l[0]%36/6+6)*12 +(data_r[0]%36/6+6);
+        }
+        
+    }
+    
+}
+
+unsigned int LineReceive(){
+    //先に要求しない場合はLineの状況を送信すること．
+    //順番適当
+    char data_r[2],data_l[2];
+    bool val;
+    //val = slave.read(address,data,length,repeat);
+    val = Sensor.read(ADDRESS_R|1, data_r, 1);
+    Led[1] = val;
+    wait_ms(5);
+    val = Sensor.read(ADDRESS_L|1, data_l, 1);
+    Led[1] = !val;
+    
+    return data_r[1]<<8 + data_l[1];
+    
+
+}
+void rotate(unsigned int times, bool dir){
+    /*
+     *回転速度，回転方向，回転角度等設定変更ののち回転．
+     *
+     *
+     *
+     */
+    
+    
+}
+
+void move(int vr, int vl){
     double pwm[4] = {0};
     uint8_t i = 0;
-    pwm[0] = (double)((vx) + vs);
-    pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
-    pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
-    pwm[3] = vk;
+    pwm[0] = vr/10.0;
+    pwm[1] = vl/10.0;
+    pwm[2] = 0;
+    pwm[3] = 0;
 
     for(i = 0; i < 4; i++){
             if(pwm[i] > 100){
@@ -78,21 +159,71 @@
     }
 }
 
+
+
 //通信（モータ用）
 void tx_motor(){
     array(speed[0],speed[1],speed[3],speed[2]);
     Motor.printf("%s",StringFIN.c_str());
 }
 
-int main() {
+void SetUp(){
+    /*初期化*/
     Motor.baud(115200);                             //ボーレート設定
     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
+    move(0,0);//停止
+
+    Step.Resets();
+    Step.ReleseSW(0,1);//記憶がない
+
+    Lcd.cls();
+    
+    /*初期化終了*/
     
     
-    move(0,0,0,0);
+}
+void StartLoop(){
+    /*
+     *スイッチが押されるまでロボットはスタートしない．
+     *
+     *待っている間にほかのスイッチが押された場合は押されている間その動作をする等.
+     *
+     *
+     */
+    
+}
+int main() {
+
+    /*IrRanking*/
+    unsigned int IrNigh;//ranking min1  > min2 > min3 > min4 
+    uint8_t IrFar;//far1 > far2;
+    
+    /*Line*/
+    unsigned int Line;
+    
+    /*Ping(仮)*/
+    
+    
+    /*move value*/
+     int  vr,vl;
+    
+    SetUp();
+    
+    StartLoop();
+    
     while(1) {
-        move(30,30,0,0);
+        
+        IrNigh = IrReceive(&IrFar);//順位設定
+        
+        /*ここにIRの場所によったプログラムを挿入*/
+        
+        /*line*/
+        Line = LineReceive();
+        
+                
+
+        move(30,30);
         wait(1.5);
 
     }