Diff: main.cpp

Revision:

0:617b63d4a532

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 27 06:01:52 2016 +0000
@@ -0,0 +1,51 @@
+#include "mbed.h"
+#include <math.h>
+#include <sstream>
+
+DigitalOut myled(LED1);
+
+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;
+
+
+//通信（モータ用）
+void tx_motor(){
+    array(speed[0],speed[1],speed[3],speed[2]);
+    motor.printf("%s",StringFIN.c_str());
+}
+
+void move(int vx, int vy){
+    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));
+    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];
+    }
+}
+int main() {
+    
+     wait(1);
+    motor.baud(115200);                             //ボーレート設定
+    motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
+    motor.attach(&tx_motor,Serial::TxIrq);          //送信空き割り込み（モータ用）
+    
+    
+    x = -10;
+    y = 10;
+    move(x,y);
+    while(1) {
+         wait(1);
+    }
+}