mbedを用いた制御学生の制御 / Mbed 2 deprecated Kouryuu_reciverv1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HMC6352 mbed

main.cpp@8:5c54756c501b, 2015-03-10 (annotated)

Committer:
WAT34
Date:
Tue Mar 10 08:59:52 2015 +0000
Revision:
8:5c54756c501b
Parent:
7:0e9cbd33abfe
Child:
10:9de2ce23c712
???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WAT34 0:2ac89e0419ac 1 #include "mbed.h"
WAT34 0:2ac89e0419ac 2 #define pi 3.141592653589793
WAT34 7:0e9cbd33abfe 3 Serial sousin(p28,p27);
WAT34 0:2ac89e0419ac 4 Serial pc(USBTX,USBRX);
WAT34 0:2ac89e0419ac 5 Timer em;
WAT34 7:0e9cbd33abfe 6 Timer t;
WAT34 7:0e9cbd33abfe 7 I2C sensor(p9,p10);
WAT34 0:2ac89e0419ac 8 DigitalOut check (LED1);
WAT34 1:252fd967389e 9 BusOut check2 (LED2,LED3);
WAT34 7:0e9cbd33abfe 10 BusOut mo1 (p15,p16);
WAT34 7:0e9cbd33abfe 11 BusOut mo2 (p17,p18);
WAT34 7:0e9cbd33abfe 12 BusOut mo3 (p19,p20);
WAT34 7:0e9cbd33abfe 13 BusIn hunter(p13,p14);
WAT34 0:2ac89e0419ac 14 PwmOut mop1 (p21);
WAT34 0:2ac89e0419ac 15 PwmOut mop2 (p22);
WAT34 0:2ac89e0419ac 16 PwmOut mop3 (p23);
WAT34 7:0e9cbd33abfe 17 double get_sen(void)
WAT34 7:0e9cbd33abfe 18 {
WAT34 7:0e9cbd33abfe 19 double degree;
WAT34 8:5c54756c501b 20 int degreed;
WAT34 7:0e9cbd33abfe 21 double phi,theta,tp;
WAT34 7:0e9cbd33abfe 22 double val[3];
WAT34 7:0e9cbd33abfe 23 int i = 0;
WAT34 7:0e9cbd33abfe 24 short d[3];
WAT34 7:0e9cbd33abfe 25 char addr = 0xD0;
WAT34 7:0e9cbd33abfe 26 char raddr = 0xD1;
WAT34 7:0e9cbd33abfe 27 char maddr = 0x18;
WAT34 7:0e9cbd33abfe 28 char mraddr = 0x19;
WAT34 7:0e9cbd33abfe 29 sensor.frequency(400000);
WAT34 7:0e9cbd33abfe 30 char put1[] = {0x6b,0x00};
WAT34 7:0e9cbd33abfe 31 char put2[] = {0x37,0x02};
WAT34 7:0e9cbd33abfe 32 char put3[] = {0x0a,0x12};
WAT34 7:0e9cbd33abfe 33 char put4[] = {0x03};
WAT34 7:0e9cbd33abfe 34 char data[6];
WAT34 7:0e9cbd33abfe 35 sensor.write(addr,put1,2);
WAT34 7:0e9cbd33abfe 36 sensor.write(addr,put2,2);
WAT34 7:0e9cbd33abfe 37 sensor.write(maddr,put3,2);
WAT34 7:0e9cbd33abfe 38 sensor.write(maddr,put4,1);
WAT34 7:0e9cbd33abfe 39 sensor.read(mraddr,data,7);
WAT34 7:0e9cbd33abfe 40 while(i<= 2){
WAT34 7:0e9cbd33abfe 41 d[i] = data[2*i] + data[2*i+1]<<8;
WAT34 7:0e9cbd33abfe 42 i++;
WAT34 7:0e9cbd33abfe 43 }
WAT34 7:0e9cbd33abfe 44 val[0] = d[0] *0.15;
WAT34 7:0e9cbd33abfe 45 val[1] = d[1] *0.15;
WAT34 7:0e9cbd33abfe 46 val[2] = d[2] *0.15;
WAT34 7:0e9cbd33abfe 47 tp = sqrt(val[0]*val[0]+val[1]*val[1]+val[2]*val[2]);
WAT34 7:0e9cbd33abfe 48 theta = atan2(val[0],val[1]);
WAT34 7:0e9cbd33abfe 49 phi = acos(val[2]/tp);
WAT34 7:0e9cbd33abfe 50 //printf("theta-->%f phi--->%f\n\r",theta,phi);
WAT34 7:0e9cbd33abfe 51 if(theta>=0)
WAT34 7:0e9cbd33abfe 52 {
WAT34 7:0e9cbd33abfe 53 degree = theta/pi*180;
WAT34 7:0e9cbd33abfe 54 }else{
WAT34 7:0e9cbd33abfe 55 degree = (pi+theta)/pi*180+180;
WAT34 7:0e9cbd33abfe 56 }
WAT34 8:5c54756c501b 57 degreed = degree;
WAT34 8:5c54756c501b 58 printf("degree-->%d\n\r",degreed);
WAT34 7:0e9cbd33abfe 59 return degree;
WAT34 7:0e9cbd33abfe 60 }
WAT34 0:2ac89e0419ac 61 double get_theta(double stickx,double sticky)
WAT34 0:2ac89e0419ac 62 {
WAT34 0:2ac89e0419ac 63 double x,y,theta;
WAT34 0:2ac89e0419ac 64 if (stickx>0.5){
WAT34 0:2ac89e0419ac 65 x = stickx - 0.5;
WAT34 0:2ac89e0419ac 66 }else if(stickx <= 0.5){
WAT34 0:2ac89e0419ac 67 x = -(0.5 - stickx);
WAT34 0:2ac89e0419ac 68 }else{
WAT34 0:2ac89e0419ac 69 x = 0;
WAT34 0:2ac89e0419ac 70 }
WAT34 0:2ac89e0419ac 71 if (sticky>0.5){
WAT34 0:2ac89e0419ac 72 y = sticky - 0.5;
WAT34 0:2ac89e0419ac 73 }else if(sticky <= 0.5){
WAT34 0:2ac89e0419ac 74 y = -(0.5 - sticky);
WAT34 0:2ac89e0419ac 75 }else{
WAT34 0:2ac89e0419ac 76 y = 0;
WAT34 0:2ac89e0419ac 77 }
WAT34 0:2ac89e0419ac 78 if(!(x==0 && y ==0)){
WAT34 0:2ac89e0419ac 79 theta =atan(y/x);
WAT34 0:2ac89e0419ac 80 }else {
WAT34 0:2ac89e0419ac 81
WAT34 0:2ac89e0419ac 82 theta = 0;
WAT34 0:2ac89e0419ac 83 }
WAT34 0:2ac89e0419ac 84 if(x>0 && y > 0){
WAT34 0:2ac89e0419ac 85 theta = pi+theta;
WAT34 0:2ac89e0419ac 86 }else
WAT34 0:2ac89e0419ac 87 if(x>0 && y < 0){
WAT34 0:2ac89e0419ac 88 theta = pi+theta;
WAT34 0:2ac89e0419ac 89 }else
WAT34 0:2ac89e0419ac 90 if(x<0 && y < 0){
WAT34 0:2ac89e0419ac 91 theta = theta;
WAT34 0:2ac89e0419ac 92 }else{
WAT34 0:2ac89e0419ac 93 theta = 2*pi+theta;
WAT34 0:2ac89e0419ac 94 }
WAT34 0:2ac89e0419ac 95 return theta;
WAT34 0:2ac89e0419ac 96 }
WAT34 0:2ac89e0419ac 97 int get_serial(double *stx,double *sty,int8_t *sw)
WAT34 0:2ac89e0419ac 98 {
WAT34 0:2ac89e0419ac 99 int error,x,y;
WAT34 0:2ac89e0419ac 100 if (sousin.getc() == 255)
WAT34 0:2ac89e0419ac 101 {
WAT34 0:2ac89e0419ac 102 x = sousin.getc();
WAT34 0:2ac89e0419ac 103 y = sousin.getc();
WAT34 0:2ac89e0419ac 104 *sw = sousin.getc();
WAT34 0:2ac89e0419ac 105 error = sousin.getc();
WAT34 0:2ac89e0419ac 106 *stx = x/100.0;
WAT34 0:2ac89e0419ac 107 *sty = y/100.0;
WAT34 0:2ac89e0419ac 108 if (error == x^y^(*sw)){
WAT34 0:2ac89e0419ac 109 em.reset();
WAT34 7:0e9cbd33abfe 110 //pc.printf("sw-->%d\n\r",x);
WAT34 0:2ac89e0419ac 111 return 0;
WAT34 0:2ac89e0419ac 112 }else{
WAT34 0:2ac89e0419ac 113 return 1;
WAT34 0:2ac89e0419ac 114 }
WAT34 0:2ac89e0419ac 115 }else{
WAT34 0:2ac89e0419ac 116 return 1;
WAT34 0:2ac89e0419ac 117 }
WAT34 0:2ac89e0419ac 118 }
WAT34 0:2ac89e0419ac 119 void allstop()
WAT34 0:2ac89e0419ac 120 {
WAT34 0:2ac89e0419ac 121 mo1 = 0;
WAT34 0:2ac89e0419ac 122 mo2 = 0;
WAT34 0:2ac89e0419ac 123 mo3 = 0;
WAT34 0:2ac89e0419ac 124 check = 1;
WAT34 0:2ac89e0419ac 125 while(1){}
WAT34 0:2ac89e0419ac 126 }
WAT34 7:0e9cbd33abfe 127 void omni_cont(int hun,double theta,int *m1,int *m2,int *m3,double *mp1,double *mp2,double *mp3)
WAT34 0:2ac89e0419ac 128 {
WAT34 0:2ac89e0419ac 129
WAT34 0:2ac89e0419ac 130 double md1,md2,md3,t1;
WAT34 0:2ac89e0419ac 131 if (hun == 2)
WAT34 0:2ac89e0419ac 132 {
WAT34 7:0e9cbd33abfe 133 t1 = 0.6;
WAT34 0:2ac89e0419ac 134 }else if(hun == 1)
WAT34 0:2ac89e0419ac 135 {
WAT34 7:0e9cbd33abfe 136 t1 = -0.6;
WAT34 0:2ac89e0419ac 137 }else{
WAT34 0:2ac89e0419ac 138 t1 = 0;
WAT34 0:2ac89e0419ac 139 }
WAT34 7:0e9cbd33abfe 140 md1 = sin(theta)*0.4+t1;
WAT34 7:0e9cbd33abfe 141 md2 = sin(theta-pi*2/3)*0.4+t1;
WAT34 7:0e9cbd33abfe 142 md3 = sin(theta-pi*4/3)*0.4+t1;
WAT34 0:2ac89e0419ac 143 if (md1 < 0){
WAT34 0:2ac89e0419ac 144 *m1 = 2;
WAT34 0:2ac89e0419ac 145 *mp1 = -md1;
WAT34 0:2ac89e0419ac 146 }else{
WAT34 0:2ac89e0419ac 147 *m1 = 1;
WAT34 0:2ac89e0419ac 148 *mp1 = md1;
WAT34 0:2ac89e0419ac 149 }
WAT34 0:2ac89e0419ac 150 if (md2 < 0){
WAT34 0:2ac89e0419ac 151 *m2 = 2;
WAT34 0:2ac89e0419ac 152 *mp2 = -md2;
WAT34 0:2ac89e0419ac 153 }else{
WAT34 0:2ac89e0419ac 154 *m2 = 1;
WAT34 0:2ac89e0419ac 155 *mp2 = md2;
WAT34 0:2ac89e0419ac 156 }
WAT34 0:2ac89e0419ac 157 if (md3 < 0){
WAT34 0:2ac89e0419ac 158 *m3 = 2;
WAT34 0:2ac89e0419ac 159 *mp3 = -md3;
WAT34 0:2ac89e0419ac 160 }else{
WAT34 0:2ac89e0419ac 161 *m3 = 1;
WAT34 0:2ac89e0419ac 162 *mp3 = md3;
WAT34 0:2ac89e0419ac 163 }
WAT34 0:2ac89e0419ac 164 }
WAT34 7:0e9cbd33abfe 165
WAT34 0:2ac89e0419ac 166 int main() {
WAT34 7:0e9cbd33abfe 167 t.start();
WAT34 0:2ac89e0419ac 168 sousin.baud(9600);
WAT34 7:0e9cbd33abfe 169 int i,ec,motor[3],sen;
WAT34 7:0e9cbd33abfe 170 double asen;
WAT34 7:0e9cbd33abfe 171 asen = get_sen();
WAT34 0:2ac89e0419ac 172 int8_t sw1,hun;
WAT34 7:0e9cbd33abfe 173 i = 0;
WAT34 0:2ac89e0419ac 174 double sx,sy,motp[3],theta;
WAT34 0:2ac89e0419ac 175 while(1) {
WAT34 6:78538ae01ce6 176 //pc.printf("START\n\r");
WAT34 7:0e9cbd33abfe 177 sen = get_sen();
WAT34 7:0e9cbd33abfe 178 hun = hunter;
WAT34 1:252fd967389e 179 check2 = hun;
WAT34 0:2ac89e0419ac 180 ec = get_serial(&sx,&sy,&sw1);
WAT34 6:78538ae01ce6 181 ec =0;
WAT34 7:0e9cbd33abfe 182 if ((hun == 1)&& (t.read_ms()>(i*200))){
WAT34 7:0e9cbd33abfe 183 asen = asen+1;
WAT34 7:0e9cbd33abfe 184 i++;
WAT34 7:0e9cbd33abfe 185 }else if((hun == 2)&& (t.read_ms()>(i*200))){
WAT34 7:0e9cbd33abfe 186 asen = asen-1;
WAT34 7:0e9cbd33abfe 187 i++;
WAT34 7:0e9cbd33abfe 188 }
WAT34 7:0e9cbd33abfe 189 if (asen > 360){
WAT34 7:0e9cbd33abfe 190 asen = 0;
WAT34 7:0e9cbd33abfe 191 }
WAT34 7:0e9cbd33abfe 192 if (asen < 0){
WAT34 7:0e9cbd33abfe 193 asen = 360;
WAT34 7:0e9cbd33abfe 194 }
WAT34 8:5c54756c501b 195 if (sen > asen+20){
WAT34 7:0e9cbd33abfe 196 hun = 1;
WAT34 8:5c54756c501b 197 }else if (sen <asen-20){
WAT34 7:0e9cbd33abfe 198 hun = 2;
WAT34 7:0e9cbd33abfe 199 }else {
WAT34 7:0e9cbd33abfe 200 hun = 0;
WAT34 7:0e9cbd33abfe 201 }
WAT34 7:0e9cbd33abfe 202 //pc.printf("asen-->%f",asen);
WAT34 7:0e9cbd33abfe 203 ec = 0;
WAT34 6:78538ae01ce6 204 //pc.printf("sx--->%dsy--->%d\n\r",sw1,sw1);
WAT34 6:78538ae01ce6 205 //pc.printf("%d\n\r",hun);
WAT34 0:2ac89e0419ac 206 if (ec == 0){
WAT34 0:2ac89e0419ac 207 em.reset();
WAT34 0:2ac89e0419ac 208 theta = get_theta(sx,sy);
WAT34 8:5c54756c501b 209 //pc.printf("%f\n\r",theta);
WAT34 0:2ac89e0419ac 210 if (sw1 == -1){
WAT34 0:2ac89e0419ac 211 omni_cont(hun,theta,&motor[0],&motor[1],&motor[2],&motp[0],&motp[1],&motp[2]);
WAT34 7:0e9cbd33abfe 212 mo1 = motor[0];
WAT34 7:0e9cbd33abfe 213 mo2 = motor[1];
WAT34 7:0e9cbd33abfe 214 mo3 = motor[2];
WAT34 7:0e9cbd33abfe 215 mop1 = motp[0];
WAT34 7:0e9cbd33abfe 216 mop2 = motp[1];
WAT34 7:0e9cbd33abfe 217 mop3 = motp[2];
WAT34 7:0e9cbd33abfe 218
WAT34 7:0e9cbd33abfe 219 }else
WAT34 7:0e9cbd33abfe 220 if (hun == 1){
WAT34 7:0e9cbd33abfe 221 mo1 = 1;
WAT34 7:0e9cbd33abfe 222 mo2 = 1;
WAT34 7:0e9cbd33abfe 223 mo3 = 1;
WAT34 8:5c54756c501b 224 mop1 = 0.5;
WAT34 8:5c54756c501b 225 mop2 = 0.5;
WAT34 8:5c54756c501b 226 mop3 = 0.5;
WAT34 7:0e9cbd33abfe 227 }else if(hun == 2){
WAT34 7:0e9cbd33abfe 228 mo1 = 2;
WAT34 7:0e9cbd33abfe 229 mo2 = 2;
WAT34 7:0e9cbd33abfe 230 mo3 = 2;
WAT34 8:5c54756c501b 231 mop1 = 0.5;
WAT34 8:5c54756c501b 232 mop2 = 0.5;
WAT34 8:5c54756c501b 233 mop3 = 0.5;
WAT34 7:0e9cbd33abfe 234 }else {
WAT34 7:0e9cbd33abfe 235 mo1 = 0;
WAT34 7:0e9cbd33abfe 236 mo2 = 0;
WAT34 7:0e9cbd33abfe 237 mo3 = 0;
WAT34 7:0e9cbd33abfe 238 mop1 = 0;
WAT34 7:0e9cbd33abfe 239 mop2 = 0;
WAT34 7:0e9cbd33abfe 240 mop3 = 0;
WAT34 0:2ac89e0419ac 241 }
WAT34 0:2ac89e0419ac 242 }else{
WAT34 0:2ac89e0419ac 243 em.start();
WAT34 7:0e9cbd33abfe 244
WAT34 0:2ac89e0419ac 245 //if(em.read_ms()>200){
WAT34 0:2ac89e0419ac 246 // allstop();
WAT34 0:2ac89e0419ac 247 //}
WAT34 0:2ac89e0419ac 248 }
WAT34 0:2ac89e0419ac 249 }
WAT34 0:2ac89e0419ac 250 }