Diff: main.cpp

Revision:

4:2857f273a7f4

Parent:

3:4a39486ff238

Child:

5:09afcbe0c18f

diff -r 4a39486ff238 -r 2857f273a7f4 main.cpp
--- a/main.cpp	Tue Mar 10 13:22:00 2015 +0000
+++ b/main.cpp	Wed Mar 11 01:03:00 2015 +0000
@@ -39,13 +39,15 @@
 #include "mbed.h"
 #include <math.h>
 #include <sstream>
+
 #include "Servo.h"
-#include "MultiSerial.h"
 #include "PID.h"
 #include "AQM0802A.h"
 #include "main.h"
 
 
+//#include "MultiSerial.h"
+/*
 void Receive(){
     IrData[0] = rx_data[0];
     IrData[1] = rx_data[1];
@@ -64,15 +66,21 @@
     
 
 }
-
+*/
 void move(int vr,int vl, double vs ,int Degree){
     double pwm[4] = {0};
     uint8_t i = 0;
-    pwm[0] = vr - vs;
-    pwm[1] = 0;
-    pwm[2] = 0;
-    pwm[3] =  vl + vs;
-
+    if(vs<0){
+        pwm[0] = vr - vs;
+        pwm[1] = 0;
+        pwm[2] = 0;
+        pwm[3] =  vl + vs;
+    }else{
+        pwm[0] = vr + vs;
+        pwm[1] = 0;
+        pwm[2] = 0;
+        pwm[3] =  vl - vs;
+    }
     for(i = 0; i < 4; i++){
             if(pwm[i] > 100){
                 pwm[i] = 100;
@@ -284,7 +292,7 @@
         }
         /*それ以外*/
         move(10,10,CompassPID,0);    
-        Receive();
+        //Receive();
         if(!(IrNum == 0)) return;
         move(40,40,CompassPID,0);
         return ;
@@ -344,12 +352,11 @@
 
 void PidUpdate()
 {   
-    Compass = rx_data[7]+rx_data[8];
     pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); 
     InputPID = Compass;
 
     pid.setProcessValue(InputPID);
-    CompassPID = 0;//-(pid.compute());
+    CompassPID = (pid.compute());
 }
 
 
@@ -401,7 +408,8 @@
     Motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     Motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
     
-
+    Mbed.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
+    
     S555.calibrate(0.0005, 120.0);
     //S555.position(0.0);    //初期位置にセット
     move(0,0,0,0);//停止
@@ -415,9 +423,7 @@
     pid.setMode(AUTO_MODE);                         //pid sed def
     pid.setSetPoint(REFERENCE);                     //pid sed def  
     pidupdate.attach(&PidUpdate, PID_CYCLE);
-    
 
- 
     
 }
 void StartLoop(){
@@ -450,7 +456,7 @@
         }
         if(State == Debug2){
              while((State == Debug2)){
-                Receive();
+                //Receive();
                 Lcd.printf("%d\n",IrNum);
                 wait_ms(100);
                 State = SwRead();
@@ -471,14 +477,13 @@
         
         if(State == Kicker){
             Led[0] = Led[1] = Led[2] = 0;
-            move(20,20,0,0);
+            Kick = 1;
+            wait_ms(500);
+            Kick = 0; 
             while((State == Kicker)){
                 wait_ms(100); 
                 State = SwRead();
-                
             }   
-            move(0,0,0,0);
-            
             continue;   
         }
     }     
@@ -519,20 +524,16 @@
     
 
     SetUp();/*set up routine*/
-    
-    //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);
     
-    while(0){
-         
+    while(1){
          
+        S555.calibrate(0.0005, 120.0);
 
-        Receive();
+        //Receive();
         //Lcd.printf("%d\n",IrNum);
         /*白線を読んでいないか確認する*/
         LineData = (~Line+0x00) & 0x0F; 
@@ -544,7 +545,7 @@
                 move(0,0,0,0);
                 while(LineIr){
                     Led[1] = Led[2] = Led[3] = 1;  
-                    Receive();
+                    //Receive();
                     LineData = (~Line+0x00) & 0x0F;
                     LineIr = LineData & IrChange[IrNum];
                     wait_ms(10);
@@ -553,7 +554,7 @@
                 move(0,0,0,0);
                 while(LinePing){
                     Led[1] = Led[2] = Led[3] = 1;  
-                    Receive();
+                    //Receive();
                     LineData = (~Line+0x00) & 0x0F;
                     LinePing = PingChange(LineData);
                     
@@ -573,7 +574,7 @@
         
         
         Led[3] = 1;
-        Receive();
+        //Receive();
         Degree = IrDegree();
         
         if((Degree == 0)||(Degree == 180)||(IrNum == 12)){
@@ -605,12 +606,14 @@
         
     }
     
-    int i;
-    while(1){
+    while(0){
     //デモプログラム
         //Receive();
+        pc.printf("%d %d %d %d %d\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1]);
+        //pc.printf("%d %d %d %d\n",PingData[1],PingData[2],PingData[3],Compass);
+        
         //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]);
+        //pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]);
         wait(0.1);
     }