T Nara / Mbed 2 deprecated robottt

ok

Dependencies:   ACM1602 mbed

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?

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