Diff: main.cpp

Revision:

0:0706f274e446

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 28 07:42:39 2014 +0000
@@ -0,0 +1,91 @@
+/*****************
+
+3輪オムニ用モータ回転計算式
+
+参考
++ http://www50.atwiki.jp/okarobocon/pages/13.html
+
++ use robocup junior program
+
+    /////////////////
++ use servo library
+
+    Servo use PwmOutPin{p21~p26}.
+///////////////////////////////////
+
+Position Servo (full left, middle, full right) = (0.0, 0.5, 1.0)
+
+
+
+
+******************/
+#include "mbed.h"
+#include <math.h>
+#include "Servo.h"
+
+
+#define MOTO_CYCLE 0.05
+Serial pc(USBTX, USBRX);
+
+Servo pwm[3] = {p21, p22, p23};
+DigitalOut myled(LED1);
+
+//Ticker Motor;
+
+void move(int vxx, int vyy){
+    /** motors print value operate***/
+    float calculate[3] = {0};
+    int i = 0;
+
+    calculate[0] = vxx *(-1.0);
+    calculate[1] = vxx *0.5 + vyy * (-(sqrt(3.0))/2.0);
+    calculate[2] = vxx *0.5 + vyy * ((sqrt(3.0))/2.0);
+    
+    for(i =0; i < 3; i++){
+        if(calculate[i] > 100){
+            calculate[i] = 100;
+        }else if(calculate[i] < -100){
+            calculate[i] = -100;
+        }
+        pwm[i] =  0.5 + (calculate[i]/ 200.0);
+    }
+    pc.printf("motor:%f, motor:%f, motor:%f\n",calculate[0],calculate[1],calculate[2]);
+    //±100を超えないように注意する
+    /*
+    pwm[0] = 0;    
+    pwm[1] = 0;
+    pwm[2] = 0;
+    */
+
+}
+
+int main() {
+    
+    //Motor.attach(&motor_printf, MOTOR_CYCLE); /*周期割り込みでモーターが動く*/
+    
+    int vx, vy;
+    float range = 0.5;
+    printf("- Position Servo (full left, middle, full right) = (0.0, 0.5, 1.0)\n");
+    move(0,0);
+    
+    //pwm[0].calibrate(range, 45.0);
+    //pwm[1].calibrate(range, 45.0);
+    //pwm[2].calibrate(range, 45.0); 
+    while(1) {
+        vx = 0;
+        vy = 0;
+        
+        /*example move*/
+         switch(pc.getc()){
+            case '1': vx = vy = -100; break;
+            case '2': vx = vy = 0; break;
+            case '3': vx = vy = 100; break;
+             
+        }
+        move(vx,vy);
+        
+        //vy = pc.gets();
+        
+        
+    }
+}