T Nara / Mbed 2 deprecated robottt

ok

Dependencies:   ACM1602 mbed

main.cpp@6:1c68c8eb8082, 2017-01-27 (annotated)

Committer:
tknara
Date:
Fri Jan 27 09:40:26 2017 +0000
Revision:
6:1c68c8eb8082
Parent:
5:714f1a772584 
get stick angle sample ;

Who changed what in which revision?

UserRevisionLine numberNew 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 5:714f1a772584 14 #define over 0.2
tknara 5:714f1a772584 15 #define under -0.2
tknara 1:daff0eea2103 16 /*header list */
tknara 1:daff0eea2103 17 #define turn 0xFF
tknara 1:daff0eea2103 18 #define velocity 0xFA
tknara 1:daff0eea2103 19 ACM1602 lcd(p28,p27,0xa0);
tknara 1:daff0eea2103 20 Serial Xbee(p13,p14);//tx rx
tknara 1:daff0eea2103 21 BusOut led(LED1,LED2,LED3,LED4);
tknara 1:daff0eea2103 22 AnalogIn sticky(p15);
tknara 0:36d4a66995e6 23 AnalogIn stickx(p16);
tknara 6:1c68c8eb8082 24 AnalogIn stickx2(p17);
tknara 6:1c68c8eb8082 25 AnalogIn sticky2(p18);
tknara 2:06791a891865 26 AnalogIn RT(p19);
tknara 2:06791a891865 27 AnalogIn LT(p20);
tknara 0:36d4a66995e6 28 Serial pc(USBTX,USBRX);
tknara 6:1c68c8eb8082 29 uint8_t GetAngle()
tknara 4:3408903ad725 30 uint8_t getdeg(float *p){
tknara 1:daff0eea2103 31 int counter = 0;
tknara 1:daff0eea2103 32 float x=0,y=0,theta=0,deg=0;
tknara 1:daff0eea2103 33 x = stickx*2;
tknara 1:daff0eea2103 34 y = sticky*2;
tknara 1:daff0eea2103 35 //printf("X == %f Y == %f\n",x,y);
tknara 1:daff0eea2103 36 if(1.1>x){
tknara 4:3408903ad725 37 x=(1-x)*xmagp;
tknara 1:daff0eea2103 38 counter +=1;
tknara 1:daff0eea2103 39 }else if(1.1<x){
tknara 4:3408903ad725 40 x=((x-1)*-1)*xmagm;
tknara 1:daff0eea2103 41 counter +=2;
tknara 5:714f1a772584 42 }if((over>x)&&(under<x)){
tknara 1:daff0eea2103 43 x = 0;
tknara 1:daff0eea2103 44 }
tknara 1:daff0eea2103 45 if(1.1>y){
tknara 4:3408903ad725 46 y=(1-y)*ymagp;
tknara 1:daff0eea2103 47 counter +=4;
tknara 1:daff0eea2103 48 }else if(1.1<y){
tknara 4:3408903ad725 49 y=((y-1)*-1)*ymagm;
tknara 1:daff0eea2103 50 counter +=8;
tknara 5:714f1a772584 51 }if((over>y)&&(under<y)){
tknara 1:daff0eea2103 52 y = 0;
tknara 1:daff0eea2103 53 }
tknara 4:3408903ad725 54 *p = x*x+y*y;
tknara 4:3408903ad725 55 if(*p>1){
tknara 4:3408903ad725 56 *p = 1;
tknara 4:3408903ad725 57 }
tknara 1:daff0eea2103 58 //printf("X == %f Y == %f\n",x,y);
tknara 1:daff0eea2103 59 theta = atan(y/x);
tknara 1:daff0eea2103 60 deg = theta * 180/pi;
tknara 1:daff0eea2103 61 //printf("%f\n",deg);
tknara 1:daff0eea2103 62 if(counter == 5){
tknara 1:daff0eea2103 63 deg = (deg-90)*-1;
tknara 1:daff0eea2103 64 led = 1;
tknara 1:daff0eea2103 65 }
tknara 1:daff0eea2103 66 else if ((counter == 6)||(counter == 10)){
tknara 1:daff0eea2103 67 deg = 270-deg;
tknara 1:daff0eea2103 68 led = 2;
tknara 1:daff0eea2103 69 }
tknara 1:daff0eea2103 70 else if(counter == 9){
tknara 1:daff0eea2103 71 deg =90-deg;
tknara 1:daff0eea2103 72 led = 4;
tknara 1:daff0eea2103 73 }
tknara 1:daff0eea2103 74 else{
tknara 1:daff0eea2103 75 deg = 0;
tknara 1:daff0eea2103 76 led = 8;
tknara 1:daff0eea2103 77 }
tknara 1:daff0eea2103 78 if((x==0)&&(y==0)){
tknara 1:daff0eea2103 79 deg = 360;
tknara 1:daff0eea2103 80 }
tknara 1:daff0eea2103 81 deg = ((uint8_t)((deg/10)+0.5))*10;
tknara 1:daff0eea2103 82 return deg/10;
tknara 1:daff0eea2103 83 }
tknara 4:3408903ad725 84
tknara 0:36d4a66995e6 85 int main() {
tknara 5:714f1a772584 86 uint8_t deg,dire2,right,left;
tknara 4:3408903ad725 87 float dire1;
tknara 6:1c68c8eb8082 88 Xbee.baud(19200);
tknara 0:36d4a66995e6 89 while(1) {
tknara 5:714f1a772584 90 right = RT*255;
tknara 5:714f1a772584 91 left = LT*255;
tknara 4:3408903ad725 92 deg = getdeg(&dire1);
tknara 4:3408903ad725 93 dire2=255*dire1;
tknara 1:daff0eea2103 94 Xbee.putc(255);
tknara 1:daff0eea2103 95 Xbee.putc(deg);
tknara 5:714f1a772584 96 Xbee.putc(right);
tknara 5:714f1a772584 97 Xbee.putc(left);
tknara 6:1c68c8eb8082 98 /*
tknara 6:1c68c8eb8082 99 Xbee.putc('\r');
tknara 6:1c68c8eb8082 100 Xbee.putc('\n');
tknara 6:1c68c8eb8082 101 */
tknara 5:714f1a772584 102 printf("%d,%d,%d,%d\n",deg,dire2,right,left);
tknara 1:daff0eea2103 103 wait(0.1);
tknara 0:36d4a66995e6 104 }
tknara 0:36d4a66995e6 105 }