Dependencies: mbed
main.cpp@2:40cfcd06ac8b, 2014-06-30 (annotated)
- Committer:
- Hatter
- Date:
- Mon Jun 30 07:27:05 2014 +0000
- Revision:
- 2:40cfcd06ac8b
- Parent:
- 1:bf0a0d529479
- Child:
- 3:cfb45044045d
- Child:
- 4:076faa2578a4
Typo
Who changed what in which revision?
User | Revision | Line number | New 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 | 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 | } |