Kiko Ishimoto
/
robocon2017_Slave_ver2
ロボットに積む
main.cpp
- Committer:
- kikoaac
- Date:
- 2017-10-13
- Revision:
- 0:06f2d82c048c
- Child:
- 1:4fb26a2daf03
File content as of revision 0:06f2d82c048c:
#include "mbed.h" #include "Servo.h" union floatInByte { unsigned short sd; unsigned char c[2]; }; Serial master(dp16,dp15); Servo arm[4] = {Servo(dp1),Servo(dp2),Servo(dp24),Servo(dp18)}; floatInByte data[4][2] = {{0,0},{0,0},{0,0},{0,0}}; char RXDATA[24] = {0,0,0,0,255,255,155,255,5,255,000,255, 0,0,0,0,255,255,155,225,5,205,000,55}; void run(int i); void rx(){ if(master.getc() == 'H'){ for(int i = 1;i < 24;i++){ RXDATA[i] = master.getc(); } for(int i = 0 ; i < 4 ; i++)run(i); } } void waitTime(float ti){ Timer t; t.start(); while(ti > t.read()); t.stop(); return; } #if 1//R #define head 4 void run(int i){ //for(int i = 0 ; i < 4 ; i ++ ){ for(int j = 0 ; j < 2; j++){ data[i][0].c[j] = RXDATA[head + i*2 + j]; } arm[i] = (float)data[i][0].sd/0xffff; //printf("%d %f ",i,float(data[i].sd)/0xffff); //} //printf("\n"); } #else//L #define head 4+12 void run(int i){ //for(int i = 0 ; i < 4 ; i ++ ){ for(int j = 0 ; j < 2; j++){ data[i][0].c[j] = RXDATA[head + i*2 + j]; } arm[i] = (float)data[i][0].sd/0xffff; //printf("%d %f %d %d ",i,float(data[i].sd)/0xffff , data[i].c[1] , data[i].c[0]); //} //printf("\n"); } #endif double armSampleTime[4] = {0,0,0,0}; bool armFlag[4] = {false,false,false,false}; void update(int i){ //armSampleTime[i] += 10.0/1000.0; double r = (float)data[i][0].sd/0xffff; double a = (float)data[i][1].sd/0xffff; double c = r + (a-r)*exp(-1.0/(10.0/1000.0)*armSampleTime[i]); printf("%5f %5f",r,a);arm[i] = r; if(fabs(r-c) < 0.001F) { data[i][1] = data[i][0]; armSampleTime[i] = 0; armFlag[i] = true; } printf("%5f ",c);//arm[i] = c; } double armOffset[4] = {0,0,0,0}; int main() { master.baud(115200); master.attach(&rx); //printf("start "); //pc.baud(115200); while(1) { //pc.printf("%x ",master.getc()); //for(int i = 0 ; i<100 ; i++){ // /* for(int i = 0 ; i < 4;i++){ update(i); run(i); } waitTime(0.01); */ wait(0.001); } }