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

アナログスティックの傾いてる角度を算出し、それにおうじてオムニの進む方向をコントロールしています。関数の使い回しはご自由にどうぞ。

Dependencies:   HMC6352 mbed

main.cpp@13:1423effc49b2, 2015-03-17 (annotated)

Committer:
WAT34
Date:
Tue Mar 17 23:43:14 2015 +0000
Revision:
13:1423effc49b2
Parent:
12:12dca00d1bca
Child:
14:54f198b9947b
?????

Who changed what in which revision?

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