ken fuji / omuni_a

oboboboboobo

Dependencies:   P_motor T_motor

Fork of omuni by gaku takasawa

omuni.cpp@2:fe76af2dff4d, 2017-10-13 (annotated)

Committer:
fujikenac
Date:
Fri Oct 13 10:10:32 2017 +0000
Revision:
2:fe76af2dff4d
Parent:
0:c0f3e14f1ed1
Child:
3:51f47bf405a0
add advanced acceleration

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 2:fe76af2dff4d 26 :i2c(i2c_),m1(i2c_,addr1, delta),m2(i2c_,addr2, delta),m3(i2c_,addr3, delta)
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 tick.attach(callback(this, &omuni::compute_task), delta);
fujikenac 2:fe76af2dff4d 34 }
fujikenac 2:fe76af2dff4d 35 void omuni::out(char rxdata[dataNum],float L)
fujikenac 2:fe76af2dff4d 36 {
fujikenac 2:fe76af2dff4d 37 signed char x1 = map(rxdata[1],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 38 signed char y1 = map(rxdata[2],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 39 signed char x2 = map(rxdata[3],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 40 signed char y2 = map(rxdata[4],-90,90,-100,100);
fujikenac 2:fe76af2dff4d 41 float r1 = pow(x1*x1+y1*y1,0.5)/100;
fujikenac 2:fe76af2dff4d 42 float r2 = pow(x2*x2+y2*y2,0.5)/100;
fujikenac 2:fe76af2dff4d 43 float sieta1 = atan2(double(x1),double(y1));
fujikenac 2:fe76af2dff4d 44 float sieta2 = atan2(double(x2),double(y2));
fujikenac 2:fe76af2dff4d 45 if(r2 < 0.1F)r2 = 0;
fujikenac 2:fe76af2dff4d 46 float alpha = PI/2;
fujikenac 2:fe76af2dff4d 47 float x_2 = cos(sieta2);
fujikenac 2:fe76af2dff4d 48 float y_2 = sin(sieta2);
fujikenac 2:fe76af2dff4d 49 float w = 0;
fujikenac 2:fe76af2dff4d 50 if(r1 > 0.1F) {
fujikenac 2:fe76af2dff4d 51 if(sieta1 > PI/3 && sieta1 < PI*2/3.0)
fujikenac 2:fe76af2dff4d 52 w = PI/4;
fujikenac 2:fe76af2dff4d 53 if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0))
fujikenac 2:fe76af2dff4d 54 w = -PI/4;
fujikenac 2:fe76af2dff4d 55 }
fujikenac 2:fe76af2dff4d 56 float s1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
fujikenac 2:fe76af2dff4d 57 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 58 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 59 m1 = s1;
fujikenac 2:fe76af2dff4d 60 m2 = s2;
fujikenac 2:fe76af2dff4d 61 m3 = s3;
fujikenac 2:fe76af2dff4d 62 //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
fujikenac 2:fe76af2dff4d 63 //printf("\n");
fujikenac 2:fe76af2dff4d 64 }
fujikenac 2:fe76af2dff4d 65
fujikenac 2:fe76af2dff4d 66 void omuni::compute_task()
fujikenac 2:fe76af2dff4d 67 {
fujikenac 2:fe76af2dff4d 68 m1.compute();
fujikenac 2:fe76af2dff4d 69 m2.compute();
fujikenac 2:fe76af2dff4d 70 m3.compute();
fujikenac 2:fe76af2dff4d 71 m1.run();
fujikenac 2:fe76af2dff4d 72 m2.run();
fujikenac 2:fe76af2dff4d 73 m3.run();
fujikenac 2:fe76af2dff4d 74 }