bousiya03 / Mbed 2 deprecated 4wd_omni

Fujisawaコードをリスペクトした4輪オムニのラフコードだよ

Dependencies:   mbed

Fork of 4wd_omni by Bチーム

main.cpp@0:9a36dbb02bc8, 2014-06-28 (annotated)

Committer:
Hatter
Date:
Sat Jun 28 06:58:19 2014 +0000
Revision:
0:9a36dbb02bc8
Fujisawa???????????4????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hatter 0:9a36dbb02bc8 1 /*
Hatter 0:9a36dbb02bc8 2 * 4WD_OMNI by Tomoki Hokida
Hatter 0:9a36dbb02bc8 3 * Every direction Type
Hatter 0:9a36dbb02bc8 4 *
Hatter 0:9a36dbb02bc8 5 */
Hatter 0:9a36dbb02bc8 6
Hatter 0:9a36dbb02bc8 7 #include "mbed.h"
Hatter 0:9a36dbb02bc8 8 #include <math.h>
Hatter 0:9a36dbb02bc8 9
Hatter 0:9a36dbb02bc8 10 #define PI 3.141592
Hatter 0:9a36dbb02bc8 11
Hatter 0:9a36dbb02bc8 12 Serial pc(USBTX,USBRX);
Hatter 0:9a36dbb02bc8 13 DigitalOut check(LED1);
Hatter 0:9a36dbb02bc8 14 PwmOut wh[4] = {p21,p22,p23,p24};
Hatter 0:9a36dbb02bc8 15
Hatter 0:9a36dbb02bc8 16 void moveOmni(int ox,int oy)
Hatter 0:9a36dbb02bc8 17 {
Hatter 0:9a36dbb02bc8 18 double trans[4] = {0};
Hatter 0:9a36dbb02bc8 19
Hatter 0:9a36dbb02bc8 20 trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI));
Hatter 0:9a36dbb02bc8 21 trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI));
Hatter 0:9a36dbb02bc8 22 trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI));
Hatter 0:9a36dbb02bc8 23 trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI));
Hatter 0:9a36dbb02bc8 24
Hatter 0:9a36dbb02bc8 25 for(int i=0;i<4;i++){
Hatter 0:9a36dbb02bc8 26
Hatter 0:9a36dbb02bc8 27 if(trans[i]>100){
Hatter 0:9a36dbb02bc8 28 trans[i]=100;
Hatter 0:9a36dbb02bc8 29 }else if(trans[i]<-100){
Hatter 0:9a36dbb02bc8 30 trans[i]=-100;
Hatter 0:9a36dbb02bc8 31 }
Hatter 0:9a36dbb02bc8 32
Hatter 0:9a36dbb02bc8 33 wh[i] = (trans[i]/100.0);
Hatter 0:9a36dbb02bc8 34 }
Hatter 0:9a36dbb02bc8 35
Hatter 0:9a36dbb02bc8 36 pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]);
Hatter 0:9a36dbb02bc8 37 }
Hatter 0:9a36dbb02bc8 38
Hatter 0:9a36dbb02bc8 39 int main()
Hatter 0:9a36dbb02bc8 40 {
Hatter 0:9a36dbb02bc8 41 int ix,iy;
Hatter 0:9a36dbb02bc8 42
Hatter 0:9a36dbb02bc8 43 for(;;){
Hatter 0:9a36dbb02bc8 44 ix=0;
Hatter 0:9a36dbb02bc8 45 iy=0;
Hatter 0:9a36dbb02bc8 46
Hatter 0:9a36dbb02bc8 47 //ex value
Hatter 0:9a36dbb02bc8 48 switch(pc.getc()){
Hatter 0:9a36dbb02bc8 49 case '1': ix = iy = -100; break;
Hatter 0:9a36dbb02bc8 50 case '2': ix = iy = 0; break;
Hatter 0:9a36dbb02bc8 51 case '3': ix = iy = 100; break;
Hatter 0:9a36dbb02bc8 52 }
Hatter 0:9a36dbb02bc8 53 moveOmni(ix,iy);
Hatter 0:9a36dbb02bc8 54
Hatter 0:9a36dbb02bc8 55 check = !check;
Hatter 0:9a36dbb02bc8 56 }
Hatter 0:9a36dbb02bc8 57
Hatter 0:9a36dbb02bc8 58 }