T Nara
/
robottt
ok
main.cpp@2:06791a891865, 2016-01-28 (annotated)
- Committer:
- tknara
- Date:
- Thu Jan 28 01:12:00 2016 +0000
- Revision:
- 2:06791a891865
- Parent:
- 1:daff0eea2103
- Child:
- 3:3ff6832ca952
sorry
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 | 1:daff0eea2103 | 10 | /*header list */ |
tknara | 1:daff0eea2103 | 11 | #define turn 0xFF |
tknara | 1:daff0eea2103 | 12 | #define velocity 0xFA |
tknara | 1:daff0eea2103 | 13 | ACM1602 lcd(p28,p27,0xa0); |
tknara | 1:daff0eea2103 | 14 | Serial Xbee(p13,p14);//tx rx |
tknara | 1:daff0eea2103 | 15 | BusOut led(LED1,LED2,LED3,LED4); |
tknara | 1:daff0eea2103 | 16 | AnalogIn sticky(p15); |
tknara | 0:36d4a66995e6 | 17 | AnalogIn stickx(p16); |
tknara | 1:daff0eea2103 | 18 | AnalogIn speed(p17); |
tknara | 2:06791a891865 | 19 | AnalogIn RT(p19); |
tknara | 2:06791a891865 | 20 | AnalogIn LT(p20); |
tknara | 0:36d4a66995e6 | 21 | Serial pc(USBTX,USBRX); |
tknara | 1:daff0eea2103 | 22 | uint8_t getdeg(){ |
tknara | 1:daff0eea2103 | 23 | int counter = 0; |
tknara | 1:daff0eea2103 | 24 | float x=0,y=0,theta=0,deg=0; |
tknara | 1:daff0eea2103 | 25 | x = stickx*2; |
tknara | 1:daff0eea2103 | 26 | y = sticky*2; |
tknara | 1:daff0eea2103 | 27 | //printf("X == %f Y == %f\n",x,y); |
tknara | 1:daff0eea2103 | 28 | if(1.1>x){ |
tknara | 1:daff0eea2103 | 29 | x=1-x; |
tknara | 1:daff0eea2103 | 30 | counter +=1; |
tknara | 1:daff0eea2103 | 31 | }else if(1.1<x){ |
tknara | 1:daff0eea2103 | 32 | x=(x-1)*-1; |
tknara | 1:daff0eea2103 | 33 | counter +=2; |
tknara | 1:daff0eea2103 | 34 | }if((0.1>x)&&(-0.1<x)){ |
tknara | 1:daff0eea2103 | 35 | x = 0; |
tknara | 1:daff0eea2103 | 36 | } |
tknara | 1:daff0eea2103 | 37 | if(1.1>y){ |
tknara | 1:daff0eea2103 | 38 | y=1-y; |
tknara | 1:daff0eea2103 | 39 | counter +=4; |
tknara | 1:daff0eea2103 | 40 | }else if(1.1<y){ |
tknara | 1:daff0eea2103 | 41 | y=(y-1)*-1; |
tknara | 1:daff0eea2103 | 42 | counter +=8; |
tknara | 1:daff0eea2103 | 43 | }if((0.1>y)&&(-0.1<y)){ |
tknara | 1:daff0eea2103 | 44 | y = 0; |
tknara | 1:daff0eea2103 | 45 | } |
tknara | 1:daff0eea2103 | 46 | //printf("X == %f Y == %f\n",x,y); |
tknara | 1:daff0eea2103 | 47 | theta = atan(y/x); |
tknara | 1:daff0eea2103 | 48 | deg = theta * 180/pi; |
tknara | 1:daff0eea2103 | 49 | //printf("%f\n",deg); |
tknara | 1:daff0eea2103 | 50 | if(counter == 5){ |
tknara | 1:daff0eea2103 | 51 | deg = (deg-90)*-1; |
tknara | 1:daff0eea2103 | 52 | led = 1; |
tknara | 1:daff0eea2103 | 53 | } |
tknara | 1:daff0eea2103 | 54 | else if ((counter == 6)||(counter == 10)){ |
tknara | 1:daff0eea2103 | 55 | deg = 270-deg; |
tknara | 1:daff0eea2103 | 56 | led = 2; |
tknara | 1:daff0eea2103 | 57 | } |
tknara | 1:daff0eea2103 | 58 | else if(counter == 9){ |
tknara | 1:daff0eea2103 | 59 | deg =90-deg; |
tknara | 1:daff0eea2103 | 60 | led = 4; |
tknara | 1:daff0eea2103 | 61 | } |
tknara | 1:daff0eea2103 | 62 | else{ |
tknara | 1:daff0eea2103 | 63 | deg = 0; |
tknara | 1:daff0eea2103 | 64 | led = 8; |
tknara | 1:daff0eea2103 | 65 | } |
tknara | 1:daff0eea2103 | 66 | if((x==0)&&(y==0)){ |
tknara | 1:daff0eea2103 | 67 | deg = 360; |
tknara | 1:daff0eea2103 | 68 | } |
tknara | 1:daff0eea2103 | 69 | deg = ((uint8_t)((deg/10)+0.5))*10; |
tknara | 1:daff0eea2103 | 70 | return deg/10; |
tknara | 1:daff0eea2103 | 71 | } |
tknara | 1:daff0eea2103 | 72 | uint8_t getspeed(){ |
tknara | 1:daff0eea2103 | 73 | float x; |
tknara | 1:daff0eea2103 | 74 | x = speed*2; |
tknara | 1:daff0eea2103 | 75 | if(1.1>x){ |
tknara | 1:daff0eea2103 | 76 | x=(1-x)*1/0.64; |
tknara | 1:daff0eea2103 | 77 | }else if(1.1<x){ |
tknara | 1:daff0eea2103 | 78 | x=(x-1)*-1*1/0.71; |
tknara | 1:daff0eea2103 | 79 | }if((0.1>x)&&(-0.1<x)){ |
tknara | 1:daff0eea2103 | 80 | x = 0; |
tknara | 1:daff0eea2103 | 81 | } |
tknara | 1:daff0eea2103 | 82 | x = (uint8_t)((0.5+x*0.5)*250); |
tknara | 1:daff0eea2103 | 83 | //printf("%f\n",x); |
tknara | 1:daff0eea2103 | 84 | return x; |
tknara | 1:daff0eea2103 | 85 | } |
tknara | 0:36d4a66995e6 | 86 | int main() { |
tknara | 2:06791a891865 | 87 | uint8_t deg,speedr,speedl; |
tknara | 0:36d4a66995e6 | 88 | while(1) { |
tknara | 2:06791a891865 | 89 | speedr = (0.7-RT)*360; |
tknara | 2:06791a891865 | 90 | speedl = (0.7-LT)*360; |
tknara | 1:daff0eea2103 | 91 | deg = getdeg(); |
tknara | 2:06791a891865 | 92 | //speed = getspeed(); |
tknara | 1:daff0eea2103 | 93 | Xbee.putc(255); |
tknara | 1:daff0eea2103 | 94 | Xbee.putc(deg); |
tknara | 2:06791a891865 | 95 | Xbee.putc(speedr); |
tknara | 2:06791a891865 | 96 | Xbee.putc(speedl); |
tknara | 2:06791a891865 | 97 | //printf("%d,%d\n",speedl,speedr); |
tknara | 1:daff0eea2103 | 98 | wait(0.1); |
tknara | 0:36d4a66995e6 | 99 | } |
tknara | 0:36d4a66995e6 | 100 | } |