Kiko Ishimoto / omuni

卒研

Dependencies:   P_motor T_motor

Fork of omuni by gaku takasawa

omuni.cpp@5:5133533ed6d2, 2017-10-16 (annotated)

Committer:
fujikenac
Date:
Mon Oct 16 09:57:14 2017 +0000
Revision:
5:5133533ed6d2
Parent:
3:51f47bf405a0
Child:
6:60f122986f29
fix

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gaku_sigu 0:c0f3e14f1ed1 1 #include "omuni.h"
gaku_sigu 0:c0f3e14f1ed1 2
gaku_sigu 0:c0f3e14f1ed1 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 }
fujikenac 2:fe76af2dff4d 23
fujikenac 2:fe76af2dff4d 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 }
fujikenac 2:fe76af2dff4d 55 float s1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
fujikenac 2:fe76af2dff4d 56 float s2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w);
fujikenac 2:fe76af2dff4d 57 float s3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w);
fujikenac 2:fe76af2dff4d 58 m1 = s1;
fujikenac 2:fe76af2dff4d 59 m2 = s2;
fujikenac 2:fe76af2dff4d 60 m3 = s3;
fujikenac 2:fe76af2dff4d 61 m1.run();
fujikenac 2:fe76af2dff4d 62 m2.run();
fujikenac 2:fe76af2dff4d 63 m3.run();
fujikenac 3:51f47bf405a0 64 //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
fujikenac 3:51f47bf405a0 65 //printf("\n");
fujikenac 2:fe76af2dff4d 66 }