T Nara / Mbed 2 deprecated robottt

ok

Dependencies:   ACM1602 mbed

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?

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 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 }