Diff: main.cpp

Revision:

3:4a39486ff238

Parent:

2:e84bb87eea71

Child:

4:2857f273a7f4

--- a/main.cpp	Tue Mar 10 08:28:15 2015 +0000
+++ b/main.cpp	Tue Mar 10 13:22:00 2015 +0000
@@ -39,10 +39,10 @@
 #include "mbed.h"
 #include <math.h>
 #include <sstream>
+#include "Servo.h"
 #include "MultiSerial.h"
 #include "PID.h"
 #include "AQM0802A.h"
-#include "Servo.h"
 #include "main.h"
 
 
@@ -54,7 +54,7 @@
     PingData[1] = rx_data[4];
     PingData[2] = rx_data[5];
     PingData[3] = rx_data[6];
-    Compass = rx_data[7]+rx_data[8];
+
     
     if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){
         IrNum = 12;
@@ -71,7 +71,7 @@
     pwm[0] = vr - vs;
     pwm[1] = 0;
     pwm[2] = 0;
-    pwm[3] =  vl*1.1 + vs;
+    pwm[3] =  vl + vs;
 
     for(i = 0; i < 4; i++){
             if(pwm[i] > 100){
@@ -253,7 +253,7 @@
 
 void IrFrontAction()//ball 12 or 0 o-clock
 {
-    if(IrData[1]>700){
+    if(IrData[1]>70){
         if(abs(CompassPID)>10){
             move(0,0,CompassPID,0);
             return;
@@ -261,7 +261,7 @@
         move(30,30,CompassPID,0);
         return;
     }
-    if(IrData[1]>600){
+    if(IrData[1]>60){
         move(25,25,CompassPID,0);
         return;
     }
@@ -303,28 +303,28 @@
      **/
     if(PingData[1]>PingData[3]){
         /*右が大きい*/
-        if(IrData[1]>700){
+        if(IrData[1]>70){
             move(-20,-20,CompassPID,45);
             return;
         }
-        if(IrData[1]>600){
+        if(IrData[1]>60){
             move(-20,-20,CompassPID,60);
             return;
         }        
-        move(-20,-20,CompassPID,90);
+        move(-20,-20,CompassPID,80);
         return;
     }
     /*左が大きい*/
     
-    if(IrData[1]>700){
+    if(IrData[1]>70){
         move(-20,-20,CompassPID,-45);
         return;
     }
-    if(IrData[1]>600){
+    if(IrData[1]>60){
         move(-20,-20,CompassPID,-60);
         return;
     }    
-    move(-20,-20,CompassPID,-90);
+    move(-20,-20,CompassPID,-80);
     return;
 
 }
@@ -332,11 +332,11 @@
 void GoHome()//Ball is not found.
 {
 
-    if(PingData[2] >=60){//後ろからの距離60cm
+    /*if(PingData[2] >=60){//後ろからの距離60cm
         move(-20,-20,CompassPID,0);
         return ;
     } 
-        
+    */     
     move(0,0,CompassPID,0);//stop
 
 }
@@ -344,12 +344,12 @@
 
 void PidUpdate()
 {   
-
+    Compass = rx_data[7]+rx_data[8];
     pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
     InputPID = Compass;
 
     pid.setProcessValue(InputPID);
-    CompassPID = -(pid.compute());
+    CompassPID = 0;//-(pid.compute());
 }
 
 
@@ -395,6 +395,8 @@
 
 void SetUp(){
     /*初期化*/
+
+    
     Motor.baud(115200);                             //ボーレート設定
     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
@@ -407,7 +409,6 @@
     Kick = 0;
     Sw.mode(PullUp);
     
-    
     pid.setInputLimits(MINIMUM,MAXIMUM);            //pid sed def
     pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);     //pid sed def
     pid.setBias(PID_BIAS);                          //pid sed def
@@ -494,7 +495,8 @@
     /*State */ 
     uint8_t LineIr = 0;
     uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06,
-                            0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
+                           
+                           0x04,0x04,0x0B,0x08,0x08,0x09,0x00};
 
     /*行動設定*/
     int Power = 0;
@@ -517,9 +519,11 @@
     
 
     SetUp();/*set up routine*/
-    Mbed.read_data(rx_data, ADDRESS);
-    Mbed.start_read();
+    
+    //Mbed.read_data(rx_data, ADDRESS);
+    //Mbed.start_read();
     //Mbed.check_rx_wait();
+    
     StartLoop(); /*loop strat, switch push end*/
     Led[0] = Led[1] = Led[2] = Led[3] = 0;
     wait_ms(100);
@@ -546,11 +550,13 @@
                     wait_ms(10);
                 }                 
             }else if(LinePing){
+                move(0,0,0,0);
                 while(LinePing){
                     Led[1] = Led[2] = Led[3] = 1;  
                     Receive();
                     LineData = (~Line+0x00) & 0x0F;
                     LinePing = PingChange(LineData);
+                    
                     wait_ms(10);
                 }
             }
@@ -602,9 +608,9 @@
     int i;
     while(1){
     //デモプログラム
-        Receive();
-        pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
-        
+        //Receive();
+        //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]);
+        pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
         wait(0.1);
     }