Diff: main.cpp

Revision:

0:e6d14fec4954

Child:

1:fb4277ce4d93

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 25 03:06:52 2014 +0000
@@ -0,0 +1,165 @@
+#include "mbed.h"
+#include "HMC6352.h"
+#include "TextLCD.h"
+#include "common.h"
+#include <math.h>
+#include <sstream>
+
+#define LINE_P 70
+#define R 1.0
+
+DigitalOut myled[4] = {LED1, LED2, LED3, LED4};
+Serial motor(p9,p10);
+Serial sensor(p13,p14);
+Serial pc(USBTX, USBRX);
+TextLCD lcd(p26, p25, p24, p23, p22, p21);
+//AnalogIn adcIn[4] = {p17, p18, p19, p20};
+//HMC6352 dcompass(p9,p10);
+
+extern string StringFIN;
+extern void array(int,int,int,int);
+extern void micon_rx(void);
+
+//uint16_t  analogHex[4] = {0};
+int speed[4] = {0};
+uint8_t ping[4] = {0};
+uint8_t line[4] = {0};
+uint8_t ir_max = 0, ir_num = 0;
+int compass = 0;
+int x = 0, y = 0, s = 0, i = 0, line_on = 0;
+int compassdef = 0, data = 0;
+
+
+void move(int vx, int vy, int vs){
+    double pwm[4] = {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] = 0;
+
+    for(i = 0; i < 4; i++){
+        if(pwm[i] > 100){
+            pwm[i] = 100;
+        } else if(pwm[i] < -100){
+            pwm[i] = -100;
+        }
+        speed[i] = pwm[i];
+    }
+}
+
+//通信（モータ用）
+void tx_motor(){
+    array(speed[0],speed[1],speed[3],speed[2]);
+    motor.printf("%s",StringFIN.c_str());
+}
+
+//ライン判断
+void line_state(){
+    if(line[0]){
+        y = -LINE_P;
+    }
+    if(line[1]){
+        x = LINE_P;
+    }
+    if(line[2]){
+        y = LINE_P;
+    }
+    if(line[3]){
+        x = -LINE_P;
+    }
+}
+
+int main() {
+    
+    //dcompass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    
+    wait(1);
+    
+    motor.baud(115200);                             //ボーレート設定
+    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
+    motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
+    sensor.attach(&micon_rx,Serial::RxIrq);         //送信空き割り込み（センサ用）
+    //compassdef = (compass / 10);           //コンパス初期値保存
+    //compassdef = (dcompass.sample() / 10);
+    
+
+    wait(1);
+
+    //while(1);
+    
+    y = 20;
+    
+    while(1){
+        s = (compass - 180) / 5;
+        //pc.printf("%d\n", s);
+        
+        move(x,y,s);
+        //wait(0.1);
+    }
+/*    
+    while(1){
+        i = 3;
+        analogHex[i] = adcIn[i].read_u16();
+        if(analogHex[i] > 10000){
+            myled[i] = 1;
+            line[i] = 1;
+            line_on = 1;
+        } else {
+            myled[i] = 0;
+            line[i] = 0;
+            line_on = 0;
+        }
+        
+        x = 30;
+        y = 0;
+                
+        if(line[1]){
+            x = LINE_P;
+        } else if(line[3]){
+            x = -LINE_P;
+        }
+        
+        if(line[0]){
+            y = -LINE_P;
+        } else if(line[2]){
+            y = LINE_P;
+        }
+        move(x,y,s);
+        
+        if(line_on)wait(0.3);
+    }
+    */
+    /*
+    while(1){
+        for(i = 0; i < 4; i++){
+            analogHex[i] = adcIn[i].read_u16();
+            if(analogHex[i] > 10000){
+                myled[i] = 1;
+                line[i] = 1;
+            } else {
+                myled[i] = 0;
+                line[i] = 0;
+            }
+            //wait(0.1);
+        }
+        
+        if(line[0]){
+            x = -30;
+        } else if(line[2]){
+            x = 30;
+        }
+        
+        if(line[1]){
+            y = 30;
+        } else if(line[3]){
+            y = -30;
+        }
+        
+        move(x,y);
+        x = 0;
+        y = 0;
+        //pc.printf("%05d   %05d   %05d   %05d\n", analogHex[0], analogHex[1], analogHex[2],  analogHex[3]);
+    }
+    */
+}