ken fuji / omuni_a

oboboboboobo

Dependencies:   P_motor T_motor

Fork of omuni by gaku takasawa

omuni.cpp@1:75b51c0b7f47, 2017-10-11 (annotated)

Committer:
fujikenac
Date:
Wed Oct 11 08:42:46 2017 +0000
Revision:
1:75b51c0b7f47
Parent:
0:c0f3e14f1ed1 
add acceleration (dev)

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 1:75b51c0b7f47 4 int8_t omuni::map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax)
fujikenac 1:75b51c0b7f47 5 {
fujikenac 1:75b51c0b7f47 6 // check it's within the range
fujikenac 1:75b51c0b7f47 7 if (inMin<inMax)
fujikenac 1:75b51c0b7f47 8 {
fujikenac 1:75b51c0b7f47 9 if (in <= inMin)
fujikenac 1:75b51c0b7f47 10 return outMin;
fujikenac 1:75b51c0b7f47 11 if (in >= inMax)
fujikenac 1:75b51c0b7f47 12 return outMax;
fujikenac 1:75b51c0b7f47 13 }
fujikenac 1:75b51c0b7f47 14 else
fujikenac 1:75b51c0b7f47 15 {
fujikenac 1:75b51c0b7f47 16 // cope with input range being backwards.
fujikenac 1:75b51c0b7f47 17 if (in >= inMin)
fujikenac 1:75b51c0b7f47 18 return outMin;
fujikenac 1:75b51c0b7f47 19 if (in <= inMax)
fujikenac 1:75b51c0b7f47 20 return outMax;
fujikenac 1:75b51c0b7f47 21 }
fujikenac 1:75b51c0b7f47 22 // calculate how far into the range we are
fujikenac 1:75b51c0b7f47 23 float scale = float(in-inMin)/float(inMax-inMin);
fujikenac 1:75b51c0b7f47 24 // calculate the output.
fujikenac 1:75b51c0b7f47 25 return int8_t(outMin + scale*float(outMax-outMin));
fujikenac 1:75b51c0b7f47 26 }
fujikenac 1:75b51c0b7f47 27
fujikenac 1:75b51c0b7f47 28 omuni::omuni(I2C* i2c_,int8_t addr1,int8_t addr2,int8_t addr3)
fujikenac 1:75b51c0b7f47 29 :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
fujikenac 1:75b51c0b7f47 30 {
fujikenac 1:75b51c0b7f47 31 RXData[0] = 'H';
fujikenac 1:75b51c0b7f47 32 RXData[1] = 0 ;
fujikenac 1:75b51c0b7f47 33 RXData[2] = 0 ;
fujikenac 1:75b51c0b7f47 34 RXData[3] = 0 ;
fujikenac 1:75b51c0b7f47 35 RXData[4] = 0 ;
fujikenac 1:75b51c0b7f47 36 }
fujikenac 1:75b51c0b7f47 37
fujikenac 1:75b51c0b7f47 38 void omuni::out(char rxdata[dataNum],float L)
fujikenac 1:75b51c0b7f47 39 {
fujikenac 1:75b51c0b7f47 40 signed char x1 = map(rxdata[1],-90,90,-100,100);
fujikenac 1:75b51c0b7f47 41 signed char y1 = map(rxdata[2],-90,90,-100,100);
fujikenac 1:75b51c0b7f47 42 signed char x2 = map(rxdata[3],-90,90,-100,100);
fujikenac 1:75b51c0b7f47 43 signed char y2 = map(rxdata[4],-90,90,-100,100);
fujikenac 1:75b51c0b7f47 44 float r1 = pow(x1*x1+y1*y1,0.5)/100;
fujikenac 1:75b51c0b7f47 45 float r2 = pow(x2*x2+y2*y2,0.5)/100;
fujikenac 1:75b51c0b7f47 46 float sieta1 = atan2(double(x1),double(y1));
fujikenac 1:75b51c0b7f47 47 float sieta2 = atan2(double(x2),double(y2));
fujikenac 1:75b51c0b7f47 48 if(r2 < 0.1F)r2 = 0;
fujikenac 1:75b51c0b7f47 49 float alpha = PI/2;
fujikenac 1:75b51c0b7f47 50 float x_2 = cos(sieta2);
fujikenac 1:75b51c0b7f47 51 float y_2 = sin(sieta2);
fujikenac 1:75b51c0b7f47 52 float w = 0;
fujikenac 1:75b51c0b7f47 53 if(r1 > 0.1F)
fujikenac 1:75b51c0b7f47 54 {
fujikenac 1:75b51c0b7f47 55 if(sieta1 > PI/3 && sieta1 < PI*2/3.0)
fujikenac 1:75b51c0b7f47 56 w = PI/4;
fujikenac 1:75b51c0b7f47 57 if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0))
fujikenac 1:75b51c0b7f47 58 w = -PI/4;
fujikenac 1:75b51c0b7f47 59 }
fujikenac 1:75b51c0b7f47 60 float s1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
fujikenac 1:75b51c0b7f47 61 float s2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w);
fujikenac 1:75b51c0b7f47 62 float s3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w);
fujikenac 1:75b51c0b7f47 63 if(s1 || s2 || s3)
fujikenac 1:75b51c0b7f47 64 acceleration();
fujikenac 1:75b51c0b7f47 65 else
fujikenac 1:75b51c0b7f47 66 speed_reset();
fujikenac 1:75b51c0b7f47 67 m1 = speed * s1;
fujikenac 1:75b51c0b7f47 68 m2 = speed * s2;
fujikenac 1:75b51c0b7f47 69 m3 = speed * s3;
fujikenac 1:75b51c0b7f47 70 //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
fujikenac 1:75b51c0b7f47 71 //printf("\n");
fujikenac 1:75b51c0b7f47 72 }
fujikenac 1:75b51c0b7f47 73
fujikenac 1:75b51c0b7f47 74 void omuni::acceleration()
fujikenac 1:75b51c0b7f47 75 {
fujikenac 1:75b51c0b7f47 76 int time = timer.read_ms();
fujikenac 1:75b51c0b7f47 77 if(time < 1000)
fujikenac 1:75b51c0b7f47 78 {
fujikenac 1:75b51c0b7f47 79 if(time == 0)
gaku_sigu 0:c0f3e14f1ed1 80 {
fujikenac 1:75b51c0b7f47 81 timer.start();
fujikenac 1:75b51c0b7f47 82 time = 10;
gaku_sigu 0:c0f3e14f1ed1 83 }
fujikenac 1:75b51c0b7f47 84 //if(time >= 1000) timer.stop();
fujikenac 1:75b51c0b7f47 85 speed = time / 1000.0f;
fujikenac 1:75b51c0b7f47 86 }
fujikenac 1:75b51c0b7f47 87 else
fujikenac 1:75b51c0b7f47 88 {
fujikenac 1:75b51c0b7f47 89 timer.stop();
fujikenac 1:75b51c0b7f47 90 speed = 1.0;
fujikenac 1:75b51c0b7f47 91 }
fujikenac 1:75b51c0b7f47 92 }
fujikenac 1:75b51c0b7f47 93
fujikenac 1:75b51c0b7f47 94 void omuni::speed_reset()
fujikenac 1:75b51c0b7f47 95 {
fujikenac 1:75b51c0b7f47 96 speed = 0;
fujikenac 1:75b51c0b7f47 97 timer.stop();
fujikenac 1:75b51c0b7f47 98 timer.reset();
fujikenac 1:75b51c0b7f47 99 }