T Nara
/
robottt
ok
main.cpp@4:3408903ad725, 2016-03-14 (annotated)
- Committer:
- tknara
- Date:
- Mon Mar 14 15:48:00 2016 +0000
- Revision:
- 4:3408903ad725
- Parent:
- 3:3ff6832ca952
- Child:
- 5:714f1a772584
no rote
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tknara | 1:daff0eea2103 | 1 | /*講習者用コントローラープログラム |
tknara | 1:daff0eea2103 | 2 | 仕様 |
tknara | 1:daff0eea2103 | 3 | 左アナログスティックでステアリングの角度 |
tknara | 1:daff0eea2103 | 4 | 右アナログスティックで速度 |
tknara | 0:36d4a66995e6 | 5 | */ |
tknara | 0:36d4a66995e6 | 6 | #include "mbed.h" |
tknara | 1:daff0eea2103 | 7 | #include "ACM1602.h" |
tknara | 1:daff0eea2103 | 8 | #define pi 3.14 |
tknara | 1:daff0eea2103 | 9 | #define rad pi/180 |
tknara | 4:3408903ad725 | 10 | #define xmagp 1.65 |
tknara | 4:3408903ad725 | 11 | #define xmagm 1.33 |
tknara | 4:3408903ad725 | 12 | #define ymagp 1.38 |
tknara | 4:3408903ad725 | 13 | #define ymagm 1.75 |
tknara | 1:daff0eea2103 | 14 | /*header list */ |
tknara | 1:daff0eea2103 | 15 | #define turn 0xFF |
tknara | 1:daff0eea2103 | 16 | #define velocity 0xFA |
tknara | 1:daff0eea2103 | 17 | ACM1602 lcd(p28,p27,0xa0); |
tknara | 1:daff0eea2103 | 18 | Serial Xbee(p13,p14);//tx rx |
tknara | 1:daff0eea2103 | 19 | BusOut led(LED1,LED2,LED3,LED4); |
tknara | 1:daff0eea2103 | 20 | AnalogIn sticky(p15); |
tknara | 0:36d4a66995e6 | 21 | AnalogIn stickx(p16); |
tknara | 1:daff0eea2103 | 22 | AnalogIn speed(p17); |
tknara | 2:06791a891865 | 23 | AnalogIn RT(p19); |
tknara | 2:06791a891865 | 24 | AnalogIn LT(p20); |
tknara | 0:36d4a66995e6 | 25 | Serial pc(USBTX,USBRX); |
tknara | 4:3408903ad725 | 26 | uint8_t getdeg(float *p){ |
tknara | 1:daff0eea2103 | 27 | int counter = 0; |
tknara | 1:daff0eea2103 | 28 | float x=0,y=0,theta=0,deg=0; |
tknara | 1:daff0eea2103 | 29 | x = stickx*2; |
tknara | 1:daff0eea2103 | 30 | y = sticky*2; |
tknara | 1:daff0eea2103 | 31 | //printf("X == %f Y == %f\n",x,y); |
tknara | 1:daff0eea2103 | 32 | if(1.1>x){ |
tknara | 4:3408903ad725 | 33 | x=(1-x)*xmagp; |
tknara | 1:daff0eea2103 | 34 | counter +=1; |
tknara | 1:daff0eea2103 | 35 | }else if(1.1<x){ |
tknara | 4:3408903ad725 | 36 | x=((x-1)*-1)*xmagm; |
tknara | 1:daff0eea2103 | 37 | counter +=2; |
tknara | 1:daff0eea2103 | 38 | }if((0.1>x)&&(-0.1<x)){ |
tknara | 1:daff0eea2103 | 39 | x = 0; |
tknara | 1:daff0eea2103 | 40 | } |
tknara | 1:daff0eea2103 | 41 | if(1.1>y){ |
tknara | 4:3408903ad725 | 42 | y=(1-y)*ymagp; |
tknara | 1:daff0eea2103 | 43 | counter +=4; |
tknara | 1:daff0eea2103 | 44 | }else if(1.1<y){ |
tknara | 4:3408903ad725 | 45 | y=((y-1)*-1)*ymagm; |
tknara | 1:daff0eea2103 | 46 | counter +=8; |
tknara | 1:daff0eea2103 | 47 | }if((0.1>y)&&(-0.1<y)){ |
tknara | 1:daff0eea2103 | 48 | y = 0; |
tknara | 1:daff0eea2103 | 49 | } |
tknara | 4:3408903ad725 | 50 | *p = x*x+y*y; |
tknara | 4:3408903ad725 | 51 | if(*p>1){ |
tknara | 4:3408903ad725 | 52 | *p = 1; |
tknara | 4:3408903ad725 | 53 | } |
tknara | 1:daff0eea2103 | 54 | //printf("X == %f Y == %f\n",x,y); |
tknara | 1:daff0eea2103 | 55 | theta = atan(y/x); |
tknara | 1:daff0eea2103 | 56 | deg = theta * 180/pi; |
tknara | 1:daff0eea2103 | 57 | //printf("%f\n",deg); |
tknara | 1:daff0eea2103 | 58 | if(counter == 5){ |
tknara | 1:daff0eea2103 | 59 | deg = (deg-90)*-1; |
tknara | 1:daff0eea2103 | 60 | led = 1; |
tknara | 1:daff0eea2103 | 61 | } |
tknara | 1:daff0eea2103 | 62 | else if ((counter == 6)||(counter == 10)){ |
tknara | 1:daff0eea2103 | 63 | deg = 270-deg; |
tknara | 1:daff0eea2103 | 64 | led = 2; |
tknara | 1:daff0eea2103 | 65 | } |
tknara | 1:daff0eea2103 | 66 | else if(counter == 9){ |
tknara | 1:daff0eea2103 | 67 | deg =90-deg; |
tknara | 1:daff0eea2103 | 68 | led = 4; |
tknara | 1:daff0eea2103 | 69 | } |
tknara | 1:daff0eea2103 | 70 | else{ |
tknara | 1:daff0eea2103 | 71 | deg = 0; |
tknara | 1:daff0eea2103 | 72 | led = 8; |
tknara | 1:daff0eea2103 | 73 | } |
tknara | 1:daff0eea2103 | 74 | if((x==0)&&(y==0)){ |
tknara | 1:daff0eea2103 | 75 | deg = 360; |
tknara | 1:daff0eea2103 | 76 | } |
tknara | 1:daff0eea2103 | 77 | deg = ((uint8_t)((deg/10)+0.5))*10; |
tknara | 1:daff0eea2103 | 78 | return deg/10; |
tknara | 1:daff0eea2103 | 79 | } |
tknara | 4:3408903ad725 | 80 | |
tknara | 0:36d4a66995e6 | 81 | int main() { |
tknara | 4:3408903ad725 | 82 | uint8_t deg,dire2; |
tknara | 4:3408903ad725 | 83 | float dire1; |
tknara | 4:3408903ad725 | 84 | Xbee.baud(9600); |
tknara | 0:36d4a66995e6 | 85 | while(1) { |
tknara | 4:3408903ad725 | 86 | deg = getdeg(&dire1); |
tknara | 4:3408903ad725 | 87 | dire2=255*dire1; |
tknara | 1:daff0eea2103 | 88 | Xbee.putc(255); |
tknara | 1:daff0eea2103 | 89 | Xbee.putc(deg); |
tknara | 4:3408903ad725 | 90 | Xbee.putc(dire2); |
tknara | 4:3408903ad725 | 91 | printf("%d,%d\n",deg,dire2); |
tknara | 1:daff0eea2103 | 92 | wait(0.1); |
tknara | 0:36d4a66995e6 | 93 | } |
tknara | 0:36d4a66995e6 | 94 | } |