Bチーム / Mbed 2 deprecated 4wd_omni

Dependencies:   mbed

main.cpp@4:076faa2578a4, 2014-07-01 (annotated)

Committer:
Hatter
Date:
Tue Jul 01 08:57:18 2014 +0000
Revision:
4:076faa2578a4
Parent:
2:40cfcd06ac8b 
pin????PWM????????????????

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