Diff: main.cpp

Revision:

1:e8b1e591f61e

Parent:

0:617b63d4a532

Child:

2:89bdaf269693

--- a/main.cpp	Sat Feb 27 06:01:52 2016 +0000
+++ b/main.cpp	Mon Mar 07 06:04:16 2016 +0000
@@ -1,16 +1,24 @@
 #include "mbed.h"
+#include "HMC6352.h"
 #include <math.h>
 #include <sstream>
 
-DigitalOut myled(LED1);
+#define PAI 3.1415926535897932384626
+#define POWER 0
+#define LIMIT 0.7
+#define TRIALS 10
+#define ZERO 0
 
+BusOut m_led(LED1,LED2,LED3,LED4);
+//DigitalOut myled(LED1);
+
+Serial pc(USBTX,USBRX);
+HMC6352 compass(p9,p10);
 Serial      motor(p13,p14);//tx,rx
 extern string StringFIN;
 extern void array(int,int,int,int);
-
 int speed[4] = {0};
-int x = 0, y = 0,i;
-
+int x = 0, y = 0,i=0,j=0;
 
 //通信（モータ用）
 void tx_motor(){
@@ -18,13 +26,17 @@
     motor.printf("%s",StringFIN.c_str());
 }
 
-void move(int vx, int vy){
+void move(double see,int direction){
     double pwm[4] = {0};
     
-    pwm[0] = 0;
-    pwm[1] = -100;//(double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy));
-    pwm[2] = //(double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy));
-    pwm[3] = 100;//(double)((vx));
+    pwm[0] = ( sin((see -  60.0)* PAI/180.0)*POWER )- direction * 0.7;
+    pwm[1] = ( sin((see - 300.0)* PAI/180.0)*POWER )- direction * 0.7;
+    pwm[2] = ( sin((see - 180.0)* PAI/180.0)*POWER )- direction * 0.7;
+    pwm[3] = 0;
+    pwm[0] *= LIMIT;
+    pwm[1] *= LIMIT;
+    pwm[2] *= LIMIT;
+    //pc.printf("%d %d %d\n",speed[0],speed[1],speed[2]);
     for(i = 0; i < 4; i++){
         if(pwm[i] > 100){
             pwm[i] = 100;
@@ -34,18 +46,58 @@
         speed[i] = pwm[i];
     }
 }
+
+int diffKun(int sampleC,int first){
+    int diff=0;
+    diff = sampleC + 540 - first;//0~180度の範囲で動かしたかった
+    diff %= 360;
+    diff -= 180;
+    return diff;
+}    
+
 int main() {
-    
-     wait(1);
-    motor.baud(115200);                             //ボーレート設定
-    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
-    motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
-    
+    int seeta=0,diff=0,min1=0,min2=1,max1=359,max2=358;
+    int sampleC[TRIALS] = {0},getCompass[TRIALS] = {0};
     
-    x = -10;
-    y = 10;
-    move(x,y);
-    while(1) {
-         wait(1);
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+    for(j=0;j<TRIALS;j++){
+        sampleC[j] = ( compass.sample() / 10.0 );
+        sampleC[j] = diffKun(sampleC[j],ZERO);
+        wait_ms(1);
+        diff += sampleC[j];
+        //pc.printf("%d ",sampleC[j]);
     }
+    diff /= TRIALS;
+    diff = diffKun(diff,ZERO);
+        
+    seeta = 0;
+    motor.baud(115200);                             //ボーレート設定
+    motor.printf("1F0002F0003F0004F000\r\n");      //モータ停止     モーター番号(1~6) + ForR(前進後進) + パワー(0~100) 
+    motor.attach(&tx_motor,Serial::TxIrq);         //送信空き割り込み（モータ用）
+    
+    //LEDは目印
+    m_led = 0x01;
+     wait(1);
+    m_led = 0x03;
+     wait(1);
+     m_led = 0x07;
+     wait(1);
+    m_led = 0x0F;
+    //
+    
+    while(1){
+         for(j=0;j<TRIALS;j++){
+            getCompass[j] = ( compass.sample() / 10.0 );
+            getCompass[j] = diffKun();
+         }
+         move(seeta,diff); 
+         pc.printf("%f %f\n",getFirst,getCompass);
+         //pc.printf("%d %d %d\n",speed[0],speed[1],speed[2]);
+         wait(0.01);
+    }
+    /*wait(1); 
+    speed[0] = 0;
+    speed[1] = 0;
+    speed[2] = 0;*/ 
+    while(1);
 }