Tk A / Move

fs

Dependents:   ARAI45th 3servotest 1stcomp

move.cpp@2:f25a09c5e113, 2016-09-05 (annotated)

Committer:
sakanakuuun
Date:
Mon Sep 05 10:37:57 2016 +0000
Revision:
2:f25a09c5e113
Parent:
1:405e28b64fdb
Child:
3:cecaa0154f92
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:d7ff86f25eaa 1 #include "mbed.h"
sakanakuuun 0:d7ff86f25eaa 2 #include "move.h"
sakanakuuun 0:d7ff86f25eaa 3 #include "locate.h"
sakanakuuun 2:f25a09c5e113 4 #include "stdlib.h"
sakanakuuun 0:d7ff86f25eaa 5
sakanakuuun 0:d7ff86f25eaa 6 PwmOut M1cw(PA_11);
sakanakuuun 0:d7ff86f25eaa 7 PwmOut M1ccw(PB_15);
sakanakuuun 0:d7ff86f25eaa 8 PwmOut M2ccw(PB_14);
sakanakuuun 0:d7ff86f25eaa 9 PwmOut M2cw(PB_13);
sakanakuuun 2:f25a09c5e113 10 /*
sakanakuuun 2:f25a09c5e113 11 DigitalOut teamledblue(PC_10);
sakanakuuun 2:f25a09c5e113 12 DigitalOut teamledred(PC_12);
sakanakuuun 2:f25a09c5e113 13 */
sakanakuuun 2:f25a09c5e113 14
sakanakuuun 2:f25a09c5e113 15 //Serial pc(SERIAL_TX, SERIAL_RX); //pcと通信
sakanakuuun 2:f25a09c5e113 16
sakanakuuun 0:d7ff86f25eaa 17
sakanakuuun 0:d7ff86f25eaa 18 //const int allowlength=5;
sakanakuuun 0:d7ff86f25eaa 19 //const float allowdegree=0.02;
sakanakuuun 2:f25a09c5e113 20 const int rightspeed=120;
sakanakuuun 2:f25a09c5e113 21 const int leftspeed=rightspeed + 2;
sakanakuuun 0:d7ff86f25eaa 22 const int turnspeed=15*2;
sakanakuuun 2:f25a09c5e113 23 const float k = 0.9;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
sakanakuuun 2:f25a09c5e113 24 const int k_theta = 2;
sakanakuuun 0:d7ff86f25eaa 25 //const float PIfact=2.89;
sakanakuuun 0:d7ff86f25eaa 26
sakanakuuun 0:d7ff86f25eaa 27
sakanakuuun 0:d7ff86f25eaa 28 void initmotor()
sakanakuuun 0:d7ff86f25eaa 29 {
sakanakuuun 0:d7ff86f25eaa 30 M1cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 31 M1ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 32 M2cw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 33 M2ccw.period_us(256);
sakanakuuun 0:d7ff86f25eaa 34
sakanakuuun 0:d7ff86f25eaa 35 }
sakanakuuun 0:d7ff86f25eaa 36
sakanakuuun 0:d7ff86f25eaa 37 void move(int left,int right)
sakanakuuun 0:d7ff86f25eaa 38 {
sakanakuuun 0:d7ff86f25eaa 39
sakanakuuun 0:d7ff86f25eaa 40 float rightduty,leftduty;
sakanakuuun 0:d7ff86f25eaa 41
sakanakuuun 0:d7ff86f25eaa 42 if(right>256) {
sakanakuuun 0:d7ff86f25eaa 43 right=256;
sakanakuuun 0:d7ff86f25eaa 44 }
sakanakuuun 0:d7ff86f25eaa 45 if(left>256) {
sakanakuuun 0:d7ff86f25eaa 46 left=256;
sakanakuuun 0:d7ff86f25eaa 47 }
sakanakuuun 0:d7ff86f25eaa 48 if(right<-256) {
sakanakuuun 0:d7ff86f25eaa 49 right=-256;
sakanakuuun 0:d7ff86f25eaa 50 }
sakanakuuun 0:d7ff86f25eaa 51 if(left<-256) {
sakanakuuun 0:d7ff86f25eaa 52 left=-256;
sakanakuuun 0:d7ff86f25eaa 53 }
sakanakuuun 0:d7ff86f25eaa 54
sakanakuuun 0:d7ff86f25eaa 55 rightduty=right/256.0;
sakanakuuun 0:d7ff86f25eaa 56 leftduty=left/256.0;
sakanakuuun 0:d7ff86f25eaa 57 if(right>0) {
sakanakuuun 0:d7ff86f25eaa 58 M1cw.write(1-rightduty);
sakanakuuun 0:d7ff86f25eaa 59 M1ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 60 } else {
sakanakuuun 0:d7ff86f25eaa 61 M1cw.write(1);
sakanakuuun 0:d7ff86f25eaa 62 M1ccw.write(1+rightduty);
sakanakuuun 0:d7ff86f25eaa 63 }
sakanakuuun 0:d7ff86f25eaa 64
sakanakuuun 0:d7ff86f25eaa 65 if(left>0) {
sakanakuuun 0:d7ff86f25eaa 66 M2cw.write(1-leftduty);
sakanakuuun 0:d7ff86f25eaa 67 M2ccw.write(1);
sakanakuuun 0:d7ff86f25eaa 68 } else {
sakanakuuun 0:d7ff86f25eaa 69 M2cw.write(1);
sakanakuuun 0:d7ff86f25eaa 70 M2ccw.write(1+leftduty);
sakanakuuun 0:d7ff86f25eaa 71 }
sakanakuuun 0:d7ff86f25eaa 72 }
sakanakuuun 0:d7ff86f25eaa 73
sakanakuuun 0:d7ff86f25eaa 74 void hosei_turn(float pt, bool cw, float rad)
sakanakuuun 0:d7ff86f25eaa 75 {
sakanakuuun 0:d7ff86f25eaa 76 int np;
sakanakuuun 0:d7ff86f25eaa 77 if(cw) np = 1;
sakanakuuun 0:d7ff86f25eaa 78 else np = -1;
sakanakuuun 0:d7ff86f25eaa 79 while(1) {
sakanakuuun 0:d7ff86f25eaa 80 update();
sakanakuuun 2:f25a09c5e113 81 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 0:d7ff86f25eaa 82 if(pt-coordinateTheta() < np * rad - ALLOW_RAD) {
sakanakuuun 2:f25a09c5e113 83 move(-10, 10);
sakanakuuun 0:d7ff86f25eaa 84 } else if(pt-coordinateTheta() > np * rad + ALLOW_RAD) {
sakanakuuun 2:f25a09c5e113 85 move(10, -10);
sakanakuuun 0:d7ff86f25eaa 86 } else {
sakanakuuun 0:d7ff86f25eaa 87 move(0,0);
sakanakuuun 0:d7ff86f25eaa 88 return;
sakanakuuun 0:d7ff86f25eaa 89 }
sakanakuuun 0:d7ff86f25eaa 90 }
sakanakuuun 2:f25a09c5e113 91
sakanakuuun 0:d7ff86f25eaa 92 }
sakanakuuun 0:d7ff86f25eaa 93
sakanakuuun 1:405e28b64fdb 94 void turn_abs_rad(float rad)
sakanakuuun 0:d7ff86f25eaa 95 {
sakanakuuun 1:405e28b64fdb 96 update();
sakanakuuun 0:d7ff86f25eaa 97
sakanakuuun 1:405e28b64fdb 98 int np;
sakanakuuun 1:405e28b64fdb 99 if(coordinateTheta() > rad) np = 1;
sakanakuuun 1:405e28b64fdb 100 else if(coordinateTheta() < rad) np = -1;
sakanakuuun 1:405e28b64fdb 101 else return;
sakanakuuun 1:405e28b64fdb 102
sakanakuuun 1:405e28b64fdb 103 move((-np)*turnspeed, (np)*turnspeed);
sakanakuuun 0:d7ff86f25eaa 104
sakanakuuun 0:d7ff86f25eaa 105 while(1) {
sakanakuuun 0:d7ff86f25eaa 106 update();
sakanakuuun 2:f25a09c5e113 107 //pc.printf("t:%f\n\r", coordinateTheta());
sakanakuuun 2:f25a09c5e113 108 if(rad - 0.2 < coordinateTheta() && coordinateTheta() < rad + 0.2) {
sakanakuuun 0:d7ff86f25eaa 109 move(0,0);
sakanakuuun 0:d7ff86f25eaa 110 break;
sakanakuuun 0:d7ff86f25eaa 111 }
sakanakuuun 0:d7ff86f25eaa 112 }
sakanakuuun 0:d7ff86f25eaa 113
sakanakuuun 1:405e28b64fdb 114
sakanakuuun 1:405e28b64fdb 115
sakanakuuun 2:f25a09c5e113 116 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 117
sakanakuuun 2:f25a09c5e113 118 wait(0.5);
sakanakuuun 1:405e28b64fdb 119
sakanakuuun 2:f25a09c5e113 120 hosei_turn(0, false, rad);
sakanakuuun 1:405e28b64fdb 121
sakanakuuun 2:f25a09c5e113 122 wait(0.5);
sakanakuuun 1:405e28b64fdb 123 }
sakanakuuun 1:405e28b64fdb 124
sakanakuuun 2:f25a09c5e113 125
sakanakuuun 2:f25a09c5e113 126 void pmove_to_dir(int direction, int x, int y)
sakanakuuun 1:405e28b64fdb 127 {
sakanakuuun 2:f25a09c5e113 128 float k = 0.9;
sakanakuuun 2:f25a09c5e113 129 int k_theta = 2;
sakanakuuun 2:f25a09c5e113 130
sakanakuuun 2:f25a09c5e113 131 int length;
sakanakuuun 2:f25a09c5e113 132 int dx, dy;
sakanakuuun 2:f25a09c5e113 133 int *d_length, *disorder;
sakanakuuun 2:f25a09c5e113 134 float dtheta;
sakanakuuun 2:f25a09c5e113 135 float ptheta;
sakanakuuun 2:f25a09c5e113 136 float daikei;
sakanakuuun 1:405e28b64fdb 137
sakanakuuun 1:405e28b64fdb 138 update();
sakanakuuun 1:405e28b64fdb 139
sakanakuuun 2:f25a09c5e113 140 switch(direction) {
sakanakuuun 2:f25a09c5e113 141 case X_PLUS:
sakanakuuun 2:f25a09c5e113 142 length = x - coordinateX();
sakanakuuun 2:f25a09c5e113 143 y = coordinateY();
sakanakuuun 2:f25a09c5e113 144 d_length = &dx;
sakanakuuun 2:f25a09c5e113 145 disorder = &dy;
sakanakuuun 2:f25a09c5e113 146 turn_abs_rad(0);
sakanakuuun 1:405e28b64fdb 147
sakanakuuun 2:f25a09c5e113 148 ptheta = 0;
sakanakuuun 2:f25a09c5e113 149 break;
sakanakuuun 2:f25a09c5e113 150 case Y_PLUS:
sakanakuuun 2:f25a09c5e113 151 length = y - coordinateY();
sakanakuuun 2:f25a09c5e113 152 x = coordinateX();
sakanakuuun 2:f25a09c5e113 153 d_length = &dy;
sakanakuuun 2:f25a09c5e113 154 disorder = &dx;
sakanakuuun 2:f25a09c5e113 155 k *= -1;
sakanakuuun 2:f25a09c5e113 156 turn_abs_rad(PI/2);
sakanakuuun 2:f25a09c5e113 157 ptheta = PI/2;
sakanakuuun 2:f25a09c5e113 158 break;
sakanakuuun 2:f25a09c5e113 159 case X_MINUS:
sakanakuuun 2:f25a09c5e113 160 length = x - coordinateX();
sakanakuuun 2:f25a09c5e113 161 y = coordinateY();
sakanakuuun 2:f25a09c5e113 162 d_length = &dx;
sakanakuuun 2:f25a09c5e113 163 disorder = &dy;
sakanakuuun 2:f25a09c5e113 164 k *= -1;
sakanakuuun 2:f25a09c5e113 165 turn_abs_rad(PI);
sakanakuuun 2:f25a09c5e113 166 //pc.printf("finish_turn");
sakanakuuun 2:f25a09c5e113 167 ptheta = PI;
sakanakuuun 2:f25a09c5e113 168 break;
sakanakuuun 2:f25a09c5e113 169 case Y_MINUS:
sakanakuuun 2:f25a09c5e113 170 length = y - coordinateY();
sakanakuuun 2:f25a09c5e113 171 x = coordinateX();
sakanakuuun 2:f25a09c5e113 172 d_length = &dy;
sakanakuuun 2:f25a09c5e113 173 disorder = &dx;
sakanakuuun 2:f25a09c5e113 174 turn_abs_rad(PI*3/2);
sakanakuuun 2:f25a09c5e113 175 ptheta = PI*3/2;
sakanakuuun 2:f25a09c5e113 176 break;
sakanakuuun 2:f25a09c5e113 177 default:
sakanakuuun 2:f25a09c5e113 178 return;
sakanakuuun 2:f25a09c5e113 179 }
sakanakuuun 1:405e28b64fdb 180
sakanakuuun 2:f25a09c5e113 181 if(length == 0) return;
sakanakuuun 1:405e28b64fdb 182
sakanakuuun 1:405e28b64fdb 183 while(1) {
sakanakuuun 1:405e28b64fdb 184 update();
sakanakuuun 2:f25a09c5e113 185 dx = coordinateX() - x;
sakanakuuun 2:f25a09c5e113 186 dy = coordinateY() - y;
sakanakuuun 2:f25a09c5e113 187 dtheta = coordinateTheta() - ptheta;
sakanakuuun 2:f25a09c5e113 188
sakanakuuun 1:405e28b64fdb 189
sakanakuuun 2:f25a09c5e113 190 if(*disorder>1) {
sakanakuuun 2:f25a09c5e113 191 *disorder = 1;
sakanakuuun 2:f25a09c5e113 192 } else if(*disorder<-1) {
sakanakuuun 2:f25a09c5e113 193 *disorder = -1;
sakanakuuun 1:405e28b64fdb 194 }
sakanakuuun 1:405e28b64fdb 195
sakanakuuun 1:405e28b64fdb 196
sakanakuuun 2:f25a09c5e113 197 if(abs(*d_length) > abs(length) -200) {
sakanakuuun 2:f25a09c5e113 198 daikei = (float)(abs(length)-abs(*d_length)) / 200;
sakanakuuun 2:f25a09c5e113 199 }
sakanakuuun 2:f25a09c5e113 200 else if(abs(*d_length) < 150) {
sakanakuuun 2:f25a09c5e113 201 daikei = (float)(abs(*d_length)) / 150;
sakanakuuun 1:405e28b64fdb 202 }
sakanakuuun 2:f25a09c5e113 203 else
sakanakuuun 2:f25a09c5e113 204 daikei = 1;
sakanakuuun 2:f25a09c5e113 205
sakanakuuun 2:f25a09c5e113 206 move(daikei * (rightspeed*7/8.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/8.0, daikei * (leftspeed*7/8.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/8.0);
sakanakuuun 2:f25a09c5e113 207
sakanakuuun 2:f25a09c5e113 208 if((direction > 0 && *d_length >= 0) || (direction < 0 && *d_length <= 0)) {
sakanakuuun 2:f25a09c5e113 209 move(0, 0);
sakanakuuun 1:405e28b64fdb 210 break;
sakanakuuun 1:405e28b64fdb 211 }
sakanakuuun 0:d7ff86f25eaa 212
sakanakuuun 2:f25a09c5e113 213 //pc.printf("d_length:%d disorder:%d rs:%f ls:%f g:%f\n\r", *d_length, *disorder, (rightspeed/2.0 - k*(*disorder) - k_theta*dtheta) + rightspeed/2.0, (leftspeed/2.0 + k*(*disorder) + k_theta*dtheta) + leftspeed/2.0, daikei);
sakanakuuun 0:d7ff86f25eaa 214 }
sakanakuuun 0:d7ff86f25eaa 215
sakanakuuun 2:f25a09c5e113 216 wait(0.5);
sakanakuuun 0:d7ff86f25eaa 217 }