Fumiya Fujisawa / Mbed 2 deprecated Temp_3legOmni

三輪オムニテンプレート的なもの，マンバマックスを通すとおそらくモーターが動かせる

Dependencies:   Servo mbed

main.cpp@0:0706f274e446, 2014-04-28 (annotated)

Committer:
ryuna
Date:
Mon Apr 28 07:42:39 2014 +0000
Revision:
0:0706f274e446
3legOmni

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryuna 0:0706f274e446 1 /*****************
ryuna 0:0706f274e446 2
ryuna 0:0706f274e446 3 3輪オムニ用モータ回転計算式
ryuna 0:0706f274e446 4
ryuna 0:0706f274e446 5 参考
ryuna 0:0706f274e446 6 + http://www50.atwiki.jp/okarobocon/pages/13.html
ryuna 0:0706f274e446 7
ryuna 0:0706f274e446 8 + use robocup junior program
ryuna 0:0706f274e446 9
ryuna 0:0706f274e446 10 /////////////////
ryuna 0:0706f274e446 11 + use servo library
ryuna 0:0706f274e446 12
ryuna 0:0706f274e446 13 Servo use PwmOutPin{p21~p26}.
ryuna 0:0706f274e446 14 ///////////////////////////////////
ryuna 0:0706f274e446 15
ryuna 0:0706f274e446 16 Position Servo (full left, middle, full right) = (0.0, 0.5, 1.0)
ryuna 0:0706f274e446 17
ryuna 0:0706f274e446 18
ryuna 0:0706f274e446 19
ryuna 0:0706f274e446 20
ryuna 0:0706f274e446 21 ******************/
ryuna 0:0706f274e446 22 #include "mbed.h"
ryuna 0:0706f274e446 23 #include <math.h>
ryuna 0:0706f274e446 24 #include "Servo.h"
ryuna 0:0706f274e446 25
ryuna 0:0706f274e446 26
ryuna 0:0706f274e446 27 #define MOTO_CYCLE 0.05
ryuna 0:0706f274e446 28 Serial pc(USBTX, USBRX);
ryuna 0:0706f274e446 29
ryuna 0:0706f274e446 30 Servo pwm[3] = {p21, p22, p23};
ryuna 0:0706f274e446 31 DigitalOut myled(LED1);
ryuna 0:0706f274e446 32
ryuna 0:0706f274e446 33 //Ticker Motor;
ryuna 0:0706f274e446 34
ryuna 0:0706f274e446 35 void move(int vxx, int vyy){
ryuna 0:0706f274e446 36 /** motors print value operate***/
ryuna 0:0706f274e446 37 float calculate[3] = {0};
ryuna 0:0706f274e446 38 int i = 0;
ryuna 0:0706f274e446 39
ryuna 0:0706f274e446 40 calculate[0] = vxx *(-1.0);
ryuna 0:0706f274e446 41 calculate[1] = vxx *0.5 + vyy * (-(sqrt(3.0))/2.0);
ryuna 0:0706f274e446 42 calculate[2] = vxx *0.5 + vyy * ((sqrt(3.0))/2.0);
ryuna 0:0706f274e446 43
ryuna 0:0706f274e446 44 for(i =0; i < 3; i++){
ryuna 0:0706f274e446 45 if(calculate[i] > 100){
ryuna 0:0706f274e446 46 calculate[i] = 100;
ryuna 0:0706f274e446 47 }else if(calculate[i] < -100){
ryuna 0:0706f274e446 48 calculate[i] = -100;
ryuna 0:0706f274e446 49 }
ryuna 0:0706f274e446 50 pwm[i] = 0.5 + (calculate[i]/ 200.0);
ryuna 0:0706f274e446 51 }
ryuna 0:0706f274e446 52 pc.printf("motor:%f, motor:%f, motor:%f\n",calculate[0],calculate[1],calculate[2]);
ryuna 0:0706f274e446 53 //±100を超えないように注意する
ryuna 0:0706f274e446 54 /*
ryuna 0:0706f274e446 55 pwm[0] = 0;
ryuna 0:0706f274e446 56 pwm[1] = 0;
ryuna 0:0706f274e446 57 pwm[2] = 0;
ryuna 0:0706f274e446 58 */
ryuna 0:0706f274e446 59
ryuna 0:0706f274e446 60 }
ryuna 0:0706f274e446 61
ryuna 0:0706f274e446 62 int main() {
ryuna 0:0706f274e446 63
ryuna 0:0706f274e446 64 //Motor.attach(&motor_printf, MOTOR_CYCLE); /*周期割り込みでモーターが動く*/
ryuna 0:0706f274e446 65
ryuna 0:0706f274e446 66 int vx, vy;
ryuna 0:0706f274e446 67 float range = 0.5;
ryuna 0:0706f274e446 68 printf("- Position Servo (full left, middle, full right) = (0.0, 0.5, 1.0)\n");
ryuna 0:0706f274e446 69 move(0,0);
ryuna 0:0706f274e446 70
ryuna 0:0706f274e446 71 //pwm[0].calibrate(range, 45.0);
ryuna 0:0706f274e446 72 //pwm[1].calibrate(range, 45.0);
ryuna 0:0706f274e446 73 //pwm[2].calibrate(range, 45.0);
ryuna 0:0706f274e446 74 while(1) {
ryuna 0:0706f274e446 75 vx = 0;
ryuna 0:0706f274e446 76 vy = 0;
ryuna 0:0706f274e446 77
ryuna 0:0706f274e446 78 /*example move*/
ryuna 0:0706f274e446 79 switch(pc.getc()){
ryuna 0:0706f274e446 80 case '1': vx = vy = -100; break;
ryuna 0:0706f274e446 81 case '2': vx = vy = 0; break;
ryuna 0:0706f274e446 82 case '3': vx = vy = 100; break;
ryuna 0:0706f274e446 83
ryuna 0:0706f274e446 84 }
ryuna 0:0706f274e446 85 move(vx,vy);
ryuna 0:0706f274e446 86
ryuna 0:0706f274e446 87 //vy = pc.gets();
ryuna 0:0706f274e446 88
ryuna 0:0706f274e446 89
ryuna 0:0706f274e446 90 }
ryuna 0:0706f274e446 91 }