ken fuji / omuni_a

oboboboboobo

Dependencies:   P_motor T_motor

Fork of omuni by gaku takasawa

omuni.cpp@0:c0f3e14f1ed1, 2017-10-10 (annotated)

Committer:
gaku_sigu
Date:
Tue Oct 10 12:50:39 2017 +0000
Revision:
0:c0f3e14f1ed1
Child:
1:75b51c0b7f47
Child:
2:fe76af2dff4d
???

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