涼太郎 中村 / Mbed 2 deprecated 3servotest

a

Dependencies:   Locate Move Servo button mbed

Fork of ARAI45th by Tk A

main.cpp@3:56b034c41dc5, 2016-09-02 (annotated)

Committer:
choutin
Date:
Fri Sep 02 06:50:14 2016 +0000
Revision:
3:56b034c41dc5
Parent:
1:10cc86cabdce
Child:
4:1604d599d40f
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sakanakuuun 0:f12d257b587e 1 #include "mbed.h"
sakanakuuun 0:f12d257b587e 2 #include "locate.h"
choutin 3:56b034c41dc5 3 #include "math.h"
choutin 3:56b034c41dc5 4 PwmOut M1cw(PA_11);
choutin 3:56b034c41dc5 5 PwmOut M1ccw(PB_15);
choutin 3:56b034c41dc5 6 PwmOut M2ccw(PB_14);
choutin 3:56b034c41dc5 7 PwmOut M2cw(PB_13);
choutin 3:56b034c41dc5 8
choutin 3:56b034c41dc5 9 const int allowlength=5;
choutin 3:56b034c41dc5 10 const float allowdegree=0.02;
choutin 3:56b034c41dc5 11 const int rightspeed=29*2;
choutin 3:56b034c41dc5 12 const int leftspeed=31*2;
choutin 3:56b034c41dc5 13 const int turnspeed=15*2;
choutin 3:56b034c41dc5 14
choutin 3:56b034c41dc5 15 const float PIfact=2.89;
choutin 3:56b034c41dc5 16
choutin 3:56b034c41dc5 17 void initmotor()
choutin 3:56b034c41dc5 18 {
choutin 3:56b034c41dc5 19
choutin 3:56b034c41dc5 20
choutin 3:56b034c41dc5 21 M1cw.period_us(256);
choutin 3:56b034c41dc5 22 M1ccw.period_us(256);
choutin 3:56b034c41dc5 23 M2cw.period_us(256);
choutin 3:56b034c41dc5 24 M2ccw.period_us(256);
choutin 3:56b034c41dc5 25
choutin 3:56b034c41dc5 26 }
choutin 3:56b034c41dc5 27
choutin 3:56b034c41dc5 28 void move(int left,int right)
choutin 3:56b034c41dc5 29 {
choutin 3:56b034c41dc5 30
choutin 3:56b034c41dc5 31 float rightduty,leftduty;
choutin 3:56b034c41dc5 32
choutin 3:56b034c41dc5 33 if(right>256) {
choutin 3:56b034c41dc5 34 right=256;
choutin 3:56b034c41dc5 35 }
choutin 3:56b034c41dc5 36 if(left>256) {
choutin 3:56b034c41dc5 37 left=256;
choutin 3:56b034c41dc5 38 }
choutin 3:56b034c41dc5 39 if(right<-256) {
choutin 3:56b034c41dc5 40 right=-256;
choutin 3:56b034c41dc5 41 }
choutin 3:56b034c41dc5 42 if(left<-256) {
choutin 3:56b034c41dc5 43 left=-256;
choutin 3:56b034c41dc5 44 }
choutin 3:56b034c41dc5 45
choutin 3:56b034c41dc5 46 rightduty=right/256.0;
choutin 3:56b034c41dc5 47 leftduty=left/256.0;
choutin 3:56b034c41dc5 48 if(right>0) {
choutin 3:56b034c41dc5 49 M1cw.write(1-rightduty);
choutin 3:56b034c41dc5 50 M1ccw.write(1);
choutin 3:56b034c41dc5 51 } else {
choutin 3:56b034c41dc5 52 M1cw.write(1);
choutin 3:56b034c41dc5 53 M1ccw.write(1+rightduty);
choutin 3:56b034c41dc5 54 }
choutin 3:56b034c41dc5 55
choutin 3:56b034c41dc5 56 if(left>0) {
choutin 3:56b034c41dc5 57 M2cw.write(1-leftduty);
choutin 3:56b034c41dc5 58 M2ccw.write(1);
choutin 3:56b034c41dc5 59 } else {
choutin 3:56b034c41dc5 60 M2cw.write(1);
choutin 3:56b034c41dc5 61 M2ccw.write(1+leftduty);
choutin 3:56b034c41dc5 62 }
choutin 3:56b034c41dc5 63 }
choutin 3:56b034c41dc5 64
choutin 3:56b034c41dc5 65
choutin 3:56b034c41dc5 66 void movelength(int length)
choutin 3:56b034c41dc5 67 {
choutin 3:56b034c41dc5 68 int px,py,pt;
choutin 3:56b034c41dc5 69 update();
choutin 3:56b034c41dc5 70 px=coordinateX();
choutin 3:56b034c41dc5 71 py=coordinateY();
choutin 3:56b034c41dc5 72 pt=coordinateTheta();
choutin 3:56b034c41dc5 73
choutin 3:56b034c41dc5 74 move(rightspeed,leftspeed);
choutin 3:56b034c41dc5 75 while(1) {
choutin 3:56b034c41dc5 76
choutin 3:56b034c41dc5 77 update();
choutin 3:56b034c41dc5 78 //pc.printf("dx:%d, dy:%d, l:%d x:%d y:%d t:%f\n\r",px-coordinateX(),py-coordinateY(),length,coordinateX(),coordinateY(), coordinateTheta());
choutin 3:56b034c41dc5 79 if(((px-coordinateX())*(px-coordinateX())+(py-coordinateY())*(py-coordinateY()))>length*length) {
choutin 3:56b034c41dc5 80 break;
choutin 3:56b034c41dc5 81 }
choutin 3:56b034c41dc5 82
choutin 3:56b034c41dc5 83 }
choutin 3:56b034c41dc5 84 move(0,0);
choutin 3:56b034c41dc5 85 }
choutin 3:56b034c41dc5 86
choutin 3:56b034c41dc5 87 void turncw()
choutin 3:56b034c41dc5 88 {
choutin 3:56b034c41dc5 89 float pt;
choutin 3:56b034c41dc5 90 move((-1)*turnspeed,turnspeed);
choutin 3:56b034c41dc5 91
choutin 3:56b034c41dc5 92 update();
choutin 3:56b034c41dc5 93 pt=coordinateTheta();
choutin 3:56b034c41dc5 94
choutin 3:56b034c41dc5 95 while(1) {
choutin 3:56b034c41dc5 96 update();
choutin 3:56b034c41dc5 97 if(pt-coordinateTheta()>PIfact/2) {
choutin 3:56b034c41dc5 98 move(0,0);
choutin 3:56b034c41dc5 99 break;
choutin 3:56b034c41dc5 100 }
choutin 3:56b034c41dc5 101 }
choutin 3:56b034c41dc5 102 }
choutin 3:56b034c41dc5 103
choutin 3:56b034c41dc5 104 void turnccw()
choutin 3:56b034c41dc5 105 {
choutin 3:56b034c41dc5 106 float pt;
choutin 3:56b034c41dc5 107 move(turnspeed,(-1)*turnspeed);
choutin 3:56b034c41dc5 108
choutin 3:56b034c41dc5 109 update();
choutin 3:56b034c41dc5 110 pt=coordinateTheta();
choutin 3:56b034c41dc5 111
choutin 3:56b034c41dc5 112 while(1) {
choutin 3:56b034c41dc5 113 update();
choutin 3:56b034c41dc5 114 if(pt-coordinateTheta()<(-1)*PIfact/2) {
choutin 3:56b034c41dc5 115 move(0,0);
choutin 3:56b034c41dc5 116 break;
choutin 3:56b034c41dc5 117 }
choutin 3:56b034c41dc5 118 }
choutin 3:56b034c41dc5 119 }
choutin 3:56b034c41dc5 120 /*
choutin 3:56b034c41dc5 121 int main(){
choutin 3:56b034c41dc5 122 setup();
choutin 3:56b034c41dc5 123 initmotor();
choutin 3:56b034c41dc5 124
choutin 3:56b034c41dc5 125 for(int i=0; i < 4; i++)
choutin 3:56b034c41dc5 126 {
choutin 3:56b034c41dc5 127 movelength(1200);
choutin 3:56b034c41dc5 128 turnccw();
choutin 3:56b034c41dc5 129 }
choutin 3:56b034c41dc5 130 }*/
choutin 3:56b034c41dc5 131
choutin 3:56b034c41dc5 132 void pmovex(int length){
choutin 3:56b034c41dc5 133 int px,py,dx,dy;
choutin 3:56b034c41dc5 134 int k=1;//P制御の係数。大きくすれば動きが大きくなる、小さくするとあまり変化しない。要はｋはP制御の感度を表す係数です。
choutin 3:56b034c41dc5 135 update();
choutin 3:56b034c41dc5 136 px=coordinateX();
choutin 3:56b034c41dc5 137 py=coordinateY();
choutin 3:56b034c41dc5 138 move(rightspeed,leftspeed);
choutin 3:56b034c41dc5 139
choutin 3:56b034c41dc5 140 while(1){
choutin 3:56b034c41dc5 141 update();
choutin 3:56b034c41dc5 142 dx=coordinateX()-px;
choutin 3:56b034c41dc5 143 dy=coordinateY()-py;
choutin 3:56b034c41dc5 144
choutin 3:56b034c41dc5 145 if(dy>7){dy=7;}
choutin 3:56b034c41dc5 146 if(dy<-7){dy=-7;}
choutin 3:56b034c41dc5 147 move(rightspeed-k*dy,leftspeed+k*dy);
choutin 3:56b034c41dc5 148
choutin 3:56b034c41dc5 149 if(dx>length){
choutin 3:56b034c41dc5 150 move(0,0);
choutin 3:56b034c41dc5 151 break;
choutin 3:56b034c41dc5 152 }
choutin 3:56b034c41dc5 153 }
choutin 3:56b034c41dc5 154 }
sakanakuuun 0:f12d257b587e 155
sakanakuuun 0:f12d257b587e 156 int main()
sakanakuuun 0:f12d257b587e 157 {
sakanakuuun 0:f12d257b587e 158 setup();
choutin 3:56b034c41dc5 159 initmotor();
sakanakuuun 0:f12d257b587e 160
choutin 3:56b034c41dc5 161
choutin 3:56b034c41dc5 162 }