Bチーム / Mbed 2 deprecated 4wd_omni

Dependencies:   mbed

main.cpp@3:cfb45044045d, 2014-06-30 (annotated)

Committer:
Hatter
Date:
Mon Jun 30 09:54:29 2014 +0000
Revision:
3:cfb45044045d
Parent:
2:40cfcd06ac8b 
simple?????????pin?????

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 1:bf0a0d529479 4 * Fujisawaリスペクトの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 3:cfb45044045d 14 DigitalOut motor[8] = {p21,p22,p23,p24,p25,p26,p27,p28};
Hatter 1:bf0a0d529479 15
Hatter 1:bf0a0d529479 16 BusInOut sw(p5, p6, p7, p8);
Hatter 1:bf0a0d529479 17
Hatter 3:cfb45044045d 18 PwmOut pwm[4] = {p17,p18,p19,p20};
Hatter 0:9a36dbb02bc8 19
Hatter 0:9a36dbb02bc8 20 void moveOmni(int ox,int oy)
Hatter 1:bf0a0d529479 21 {
Hatter 1:bf0a0d529479 22
Hatter 0:9a36dbb02bc8 23 double trans[4] = {0};
Hatter 0:9a36dbb02bc8 24
Hatter 0:9a36dbb02bc8 25 trans[0] = ox*(cos(0.75*PI)) + oy*(sin(0.75*PI));
Hatter 0:9a36dbb02bc8 26 trans[1] = ox*(cos(-0.75*PI)) + oy*(sin(-0.75*PI));
Hatter 0:9a36dbb02bc8 27 trans[2] = ox*(cos(-0.25*PI)) + oy*(sin(-0.25*PI));
Hatter 0:9a36dbb02bc8 28 trans[3] = ox*(cos(0.25*PI)) + oy*(sin(0.25*PI));
Hatter 0:9a36dbb02bc8 29
Hatter 0:9a36dbb02bc8 30 for(int i=0;i<4;i++){
Hatter 0:9a36dbb02bc8 31
Hatter 1:bf0a0d529479 32 if(trans[i]>0){
Hatter 1:bf0a0d529479 33 motor[i*2] = 1;
Hatter 1:bf0a0d529479 34 motor[i*2+1] = 0;
Hatter 1:bf0a0d529479 35 }else{
Hatter 1:bf0a0d529479 36 motor[i*2+1] = 1;
Hatter 1:bf0a0d529479 37 motor[i*2] = 0;
Hatter 1:bf0a0d529479 38 }
Hatter 1:bf0a0d529479 39 trans[i] = fabs(trans[i]);
Hatter 1:bf0a0d529479 40 if(trans[i]>100){
Hatter 1:bf0a0d529479 41 trans[i]=100;}
Hatter 1:bf0a0d529479 42 pwm[i] = (trans[i]/100.0);
Hatter 0:9a36dbb02bc8 43 }
Hatter 0:9a36dbb02bc8 44
Hatter 1:bf0a0d529479 45
Hatter 0:9a36dbb02bc8 46 pc.printf("motor:%f, motor:%f, motor:%f, motor:%f\n",trans[0],trans[1],trans[2],trans[3]);
Hatter 1:bf0a0d529479 47
Hatter 1:bf0a0d529479 48 }
Hatter 0:9a36dbb02bc8 49
Hatter 0:9a36dbb02bc8 50 int main()
Hatter 0:9a36dbb02bc8 51 {
Hatter 0:9a36dbb02bc8 52 int ix,iy;
Hatter 0:9a36dbb02bc8 53
Hatter 0:9a36dbb02bc8 54 for(;;){
Hatter 0:9a36dbb02bc8 55 ix=0;
Hatter 0:9a36dbb02bc8 56 iy=0;
Hatter 1:bf0a0d529479 57
Hatter 1:bf0a0d529479 58 sw.input();
Hatter 1:bf0a0d529479 59 sw.mode(PullUp);
Hatter 1:bf0a0d529479 60
Hatter 0:9a36dbb02bc8 61 //ex value
Hatter 1:bf0a0d529479 62 switch(sw){
Hatter 1:bf0a0d529479 63 case 0xF: ix = iy = 0; break;
Hatter 1:bf0a0d529479 64 case 0xE: ix = iy = 100; break;
Hatter 1:bf0a0d529479 65 case 0xD: ix = iy = 50; break;
Hatter 1:bf0a0d529479 66 case 0xB: ix = iy = -100; break;
Hatter 1:bf0a0d529479 67 case 0x7: ix = iy = -50; break;
Hatter 1:bf0a0d529479 68 }
Hatter 1:bf0a0d529479 69
Hatter 0:9a36dbb02bc8 70 moveOmni(ix,iy);
Hatter 0:9a36dbb02bc8 71
Hatter 0:9a36dbb02bc8 72 check = !check;
Hatter 0:9a36dbb02bc8 73 }
Hatter 0:9a36dbb02bc8 74
Hatter 0:9a36dbb02bc8 75 }