T Nara / Mbed 2 deprecated robottt

ok

Dependencies:   ACM1602 mbed

main.cpp@1:daff0eea2103, 2016-01-23 (annotated)

Committer:
tknara
Date:
Sat Jan 23 08:12:01 2016 +0000
Revision:
1:daff0eea2103
Parent:
0:36d4a66995e6
Child:
2:06791a891865
ok;

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 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 0:36d4a66995e6 19 Serial pc(USBTX,USBRX);
tknara 1:daff0eea2103 20 uint8_t getdeg(){
tknara 1:daff0eea2103 21 int counter = 0;
tknara 1:daff0eea2103 22 float x=0,y=0,theta=0,deg=0;
tknara 1:daff0eea2103 23 x = stickx*2;
tknara 1:daff0eea2103 24 y = sticky*2;
tknara 1:daff0eea2103 25 //printf("X == %f Y == %f\n",x,y);
tknara 1:daff0eea2103 26 if(1.1>x){
tknara 1:daff0eea2103 27 x=1-x;
tknara 1:daff0eea2103 28 counter +=1;
tknara 1:daff0eea2103 29 }else if(1.1<x){
tknara 1:daff0eea2103 30 x=(x-1)*-1;
tknara 1:daff0eea2103 31 counter +=2;
tknara 1:daff0eea2103 32 }if((0.1>x)&&(-0.1<x)){
tknara 1:daff0eea2103 33 x = 0;
tknara 1:daff0eea2103 34 }
tknara 1:daff0eea2103 35 if(1.1>y){
tknara 1:daff0eea2103 36 y=1-y;
tknara 1:daff0eea2103 37 counter +=4;
tknara 1:daff0eea2103 38 }else if(1.1<y){
tknara 1:daff0eea2103 39 y=(y-1)*-1;
tknara 1:daff0eea2103 40 counter +=8;
tknara 1:daff0eea2103 41 }if((0.1>y)&&(-0.1<y)){
tknara 1:daff0eea2103 42 y = 0;
tknara 1:daff0eea2103 43 }
tknara 1:daff0eea2103 44 //printf("X == %f Y == %f\n",x,y);
tknara 1:daff0eea2103 45 theta = atan(y/x);
tknara 1:daff0eea2103 46 deg = theta * 180/pi;
tknara 1:daff0eea2103 47 //printf("%f\n",deg);
tknara 1:daff0eea2103 48 if(counter == 5){
tknara 1:daff0eea2103 49 deg = (deg-90)*-1;
tknara 1:daff0eea2103 50 led = 1;
tknara 1:daff0eea2103 51 }
tknara 1:daff0eea2103 52 else if ((counter == 6)||(counter == 10)){
tknara 1:daff0eea2103 53 deg = 270-deg;
tknara 1:daff0eea2103 54 led = 2;
tknara 1:daff0eea2103 55 }
tknara 1:daff0eea2103 56 else if(counter == 9){
tknara 1:daff0eea2103 57 deg =90-deg;
tknara 1:daff0eea2103 58 led = 4;
tknara 1:daff0eea2103 59 }
tknara 1:daff0eea2103 60 else{
tknara 1:daff0eea2103 61 deg = 0;
tknara 1:daff0eea2103 62 led = 8;
tknara 1:daff0eea2103 63 }
tknara 1:daff0eea2103 64 if((x==0)&&(y==0)){
tknara 1:daff0eea2103 65 deg = 360;
tknara 1:daff0eea2103 66 }
tknara 1:daff0eea2103 67 deg = ((uint8_t)((deg/10)+0.5))*10;
tknara 1:daff0eea2103 68 return deg/10;
tknara 1:daff0eea2103 69 }
tknara 1:daff0eea2103 70 uint8_t getspeed(){
tknara 1:daff0eea2103 71 float x;
tknara 1:daff0eea2103 72 x = speed*2;
tknara 1:daff0eea2103 73 if(1.1>x){
tknara 1:daff0eea2103 74 x=(1-x)*1/0.64;
tknara 1:daff0eea2103 75 }else if(1.1<x){
tknara 1:daff0eea2103 76 x=(x-1)*-1*1/0.71;
tknara 1:daff0eea2103 77 }if((0.1>x)&&(-0.1<x)){
tknara 1:daff0eea2103 78 x = 0;
tknara 1:daff0eea2103 79 }
tknara 1:daff0eea2103 80 x = (uint8_t)((0.5+x*0.5)*250);
tknara 1:daff0eea2103 81 //printf("%f\n",x);
tknara 1:daff0eea2103 82 return x;
tknara 1:daff0eea2103 83 }
tknara 0:36d4a66995e6 84 int main() {
tknara 1:daff0eea2103 85 uint8_t deg,speed;
tknara 0:36d4a66995e6 86 while(1) {
tknara 1:daff0eea2103 87 deg = getdeg();
tknara 1:daff0eea2103 88 speed = getspeed();
tknara 1:daff0eea2103 89 Xbee.putc(255);
tknara 1:daff0eea2103 90 Xbee.putc(deg);
tknara 1:daff0eea2103 91 Xbee.putc(speed);
tknara 1:daff0eea2103 92 printf("%d\n",deg);
tknara 1:daff0eea2103 93 wait(0.1);
tknara 0:36d4a66995e6 94 }
tknara 0:36d4a66995e6 95 }