Kiko Ishimoto / omuni

卒研

Dependencies:   P_motor T_motor

Fork of omuni by gaku takasawa

omuni.cpp@6:60f122986f29, 2017-10-25 (annotated)

Committer:
gaku_sigu
Date:
Wed Oct 25 08:53:17 2017 +0000
Revision:
6:60f122986f29
Parent:
5:5133533ed6d2
Child:
7:76e11784ce6b
bobo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gaku_sigu 0:c0f3e14f1ed1 1 #include "omuni.h"
gaku_sigu 6:60f122986f29 2
gaku_sigu 6:60f122986f29 3
fujikenac 2:fe76af2dff4d 4 int8_t omuni::map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax)
fujikenac 2:fe76af2dff4d 5 {
fujikenac 2:fe76af2dff4d 6 // check it's within the range
fujikenac 2:fe76af2dff4d 7 if (inMin<inMax) {
fujikenac 2:fe76af2dff4d 8 if (in <= inMin)
fujikenac 2:fe76af2dff4d 9 return outMin;
fujikenac 2:fe76af2dff4d 10 if (in >= inMax)
fujikenac 2:fe76af2dff4d 11 return outMax;
fujikenac 2:fe76af2dff4d 12 } else { // cope with input range being backwards.
fujikenac 2:fe76af2dff4d 13 if (in >= inMin)
fujikenac 2:fe76af2dff4d 14 return outMin;
fujikenac 2:fe76af2dff4d 15 if (in <= inMax)
fujikenac 2:fe76af2dff4d 16 return outMax;
fujikenac 2:fe76af2dff4d 17 }
fujikenac 2:fe76af2dff4d 18 // calculate how far into the range we are
fujikenac 2:fe76af2dff4d 19 float scale = float(in-inMin)/float(inMax-inMin);
fujikenac 2:fe76af2dff4d 20 // calculate the output.
fujikenac 2:fe76af2dff4d 21 return int8_t(outMin + scale*float(outMax-outMin));
fujikenac 2:fe76af2dff4d 22 }
gaku_sigu 6:60f122986f29 23
gaku_sigu 6:60f122986f29 24
fujikenac 2:fe76af2dff4d 25 omuni::omuni(I2C* i2c_,int8_t addr1,int8_t addr2,int8_t addr3, double delta)
fujikenac 5:5133533ed6d2 26 :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
fujikenac 2:fe76af2dff4d 27 {
fujikenac 2:fe76af2dff4d 28 RXData[0] = 'H';
fujikenac 2:fe76af2dff4d 29 RXData[1] = 0 ;
fujikenac 2:fe76af2dff4d 30 RXData[2] = 0 ;
fujikenac 2:fe76af2dff4d 31 RXData[3] = 0 ;
fujikenac 2:fe76af2dff4d 32 RXData[4] = 0 ;
fujikenac 2:fe76af2dff4d 33 }
fujikenac 2:fe76af2dff4d 34 void omuni::out(char rxdata[dataNum],float L)
fujikenac 2:fe76af2dff4d 35 {
fujikenac 2:fe76af2dff4d 36 signed char x1 = map(rxdata[1],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 37 signed char y1 = map(rxdata[2],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 38 signed char x2 = map(rxdata[3],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 39 signed char y2 = map(rxdata[4],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 40 float r1 = pow(x1*x1+y1*y1,0.5)/100;
fujikenac 2:fe76af2dff4d 41 float r2 = pow(x2*x2+y2*y2,0.5)/100;
fujikenac 2:fe76af2dff4d 42 float sieta1 = atan2(double(x1),double(y1));
fujikenac 2:fe76af2dff4d 43 float sieta2 = atan2(double(x2),double(y2));
fujikenac 2:fe76af2dff4d 44 if(r2 < 0.1F)r2 = 0;
fujikenac 2:fe76af2dff4d 45 float alpha = PI/2;
fujikenac 2:fe76af2dff4d 46 float x_2 = cos(sieta2);
fujikenac 2:fe76af2dff4d 47 float y_2 = sin(sieta2);
fujikenac 2:fe76af2dff4d 48 float w = 0;
fujikenac 2:fe76af2dff4d 49 if(r1 > 0.1F) {
fujikenac 2:fe76af2dff4d 50 if(sieta1 > PI/3 && sieta1 < PI*2/3.0)
fujikenac 5:5133533ed6d2 51 w = PI/3;
fujikenac 2:fe76af2dff4d 52 if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0))
fujikenac 5:5133533ed6d2 53 w = -PI/3;
fujikenac 2:fe76af2dff4d 54 }
gaku_sigu 6:60f122986f29 55
gaku_sigu 6:60f122986f29 56 if(r2)
gaku_sigu 6:60f122986f29 57 {
gaku_sigu 6:60f122986f29 58 m1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
gaku_sigu 6:60f122986f29 59 m2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w);
gaku_sigu 6:60f122986f29 60 m3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w);
gaku_sigu 6:60f122986f29 61 m1.run();
gaku_sigu 6:60f122986f29 62 m2.run();
gaku_sigu 6:60f122986f29 63 m3.run();
gaku_sigu 6:60f122986f29 64 }
gaku_sigu 6:60f122986f29 65 else
gaku_sigu 6:60f122986f29 66 {
gaku_sigu 6:60f122986f29 67 float spdt = 0.5f * float(L*w);
gaku_sigu 6:60f122986f29 68 m1.direct_controll(spdt);
gaku_sigu 6:60f122986f29 69 m2.direct_controll(spdt);
gaku_sigu 6:60f122986f29 70 m3.direct_controll(spdt);
gaku_sigu 6:60f122986f29 71 }
fujikenac 3:51f47bf405a0 72 //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
fujikenac 3:51f47bf405a0 73 //printf("\n");
fujikenac 2:fe76af2dff4d 74 }