ai2001.rar

/* Apply UVC to KHR. May 8,2022 : Vre 1.0 */ #include "stdio.h" #include "kcb5.h" #include "i2c.h" #include "uart.h" #include "pio.h" #include "rom.h" #include "timer.h" #include "ics.h" #include "ad.h" #include "dac.h" #include "math.h" #define OFS_L 129.0 //初期の股関節までの距離（膝カク防止） #define OFS_S 213 //初期の股関節サーボ角 #define CHG_SVA 1718.9 //サーボ角に変換 #define HEIGHT 185 //185脚長 #define RET_F_LEG 3 //遊脚追従率 #define RST_F_LEG 3 //軸足垂直戻率 //// グローバル変数 int16_t K0W[2]; //股関節前後方向書込用 int16_t K1W[2]; //股関節横方向書込用 int16_t K2W[2]; //股関節横方向書込用 int16_t HW [2]; //膝関節書込用 int16_t A0W[2]; //足首上下方向書込用 int16_t A1W[2]; //足首横方向書込用 int16_t U0W[2]; //肩前後方向書込用 int16_t U1W[2]; //肩横後方向書込用 int16_t U2W[2]; //肩ヨー向書込用 int16_t EW[2]; //肘書込用 int16_t WESTW; //腰回転書込用 int16_t HEADW; //頭回転書込用 int16_t K0R,K0RB,U0R,U0L,U1R,U1L,EWS; int16_t K0L,K0LB,U0WB; int16_t K0WB; int16_t jikuasi; int16_t motCt,motCtBak,motCtBak2,motCtdat; int16_t mode,modeNxt,subMode,keyMode; int16_t pitch_gyr,roll_gyr,yaw_gyr; int16_t cycle,tst0; int16_t walkCt,walkCtLim; //歩数 int16_t p_ofs,r_ofs; int16_t ir,ip,ira,ipa; int16_t irb,ipb,ct; int16_t pitchs,rolls,pitch_ofs, roll_ofs, yaw, yaw_ofs; int16_t landF,landB; int32_t tBak, pitchi; uint32_t tNow; uint8_t cmd[2]; uint8_t ff[45]; uint8_t dsp[110]; uint8_t krr[4]; int8_t kn; int8_t LEDct; //LED点灯カウンタ float fwctEnd,fwct,fwctUp; float pitch,roll,pitcht,rollt; float pitch_gyrg, roll_gyrg; float wk,wt; float dyi,dyib,dyis; float dxi,dxib,dxis; float rollg,fw,fwi,fws,sw,freeBak,tt0; float supportingLeg,swingLeg; //支持脚、遊脚股関節振出角度 float footH; //脚上げ高さ float swx,swy,swMax; //横振り巾 float autoH,fh,fhMax; //脚上げ高さ //// 汎用 //// int32_t ii; int16_t i,j; float k,k0,k1,ks,kxy,kl; //////////////////////// //// ターミナル表示 //// //////////////////////// void printS(char *StringData){ unsigned char count = 0; while(StringData[count] != '\0')count++; uart_tx(UART_COM, (unsigned char*)StringData, 0, count); } //////////////////////// //// 遅延時間の設定 //// //////////////////////// void delay(int32_t t) { int32_t tb=0; t*=1000; timer_write(TIMER,t); timer_start(TIMER); t=0; while(t >= tb){ tb=t; t=timer_read(TIMER); } } ///////////////////// //// I2Cアクセス //// ///////////////////// uint8_t read8(uint8_t reg ) { cmd[0]=reg; //アドレスを0x28から左に1bitシフトし0x50、KONDOのI2Cライブラリバグ対応 i2c_read(0x50, cmd, 1, ff, 1); //BNO055_CHIP_ID_ADDR(0x28) return ff[0]; } bool readLen(uint8_t reg, uint8_t len) { cmd[0]=reg; //アドレスを0x28から左に1bitシフトし0x50、KONDOのI2Cライブラリバグ対応 i2c_read(0x50, cmd, 1, ff, len); //BNO055_CHIP_ID_ADDR(0x28) return true; } bool write8(uint8_t reg, uint8_t dat) { cmd[0]=dat; //アドレスを0x28から左に1bitシフトし0x50、KONDOのI2Cライブラリバグ対応 i2c_write(0x50, reg, cmd, 1); //BNO055_CHIP_ID_ADDR(0x28) return true; } ///////////////////////////////////// //// 目標値まで徐々に間接を動かす //// ///////////////////////////////////// void movSv(short *s,int d){ //引数 s:現サーボ位置 d:目標サーボ位置 if(motCt<1) *s = d; else *s += (d-*s)/motCt; } /////////////////////////// //// 検出角度を校正する //// /////////////////////////// void angAdj(void){ // **** pitch & roll **** if( pitchs<=ipb+1 && pitchs>=ipb-1 && rolls <=irb+1 && rolls >=irb-1 ){ ++ip; ipa+=pitchs; ira+=rolls; } else { ip=0; ipa=0; ira=0; } ipb=pitchs; irb=rolls; sprintf( (char *)dsp,"P:%4d R:%4d C:%4d\r",pitchs,rolls,ip ); printS((char *)dsp); } /////////////////////// //// 全方向転倒検知 //// /////////////////////// void detAng(void){ if( 0.35>fabs(pitch) && 0.35>fabs(roll) )return; sprintf( (char *)dsp," PRA:%4d %4d PRG:%4d %4d\r\n",pitchs,rolls,pitch_gyr,roll_gyr ); printS((char *)dsp); ics_set_pos ( UART_SIO2, 1, 4735 +1350 ); //U0R ics_set_pos ( UART_SIO4, 1, 9320 -1350 ); //U0L ics_set_pos ( UART_SIO2, 4, 4800 +2700 ); //ER ics_set_pos ( UART_SIO4, 4,10150 -2700 ); //EL ics_set_pos ( UART_SIO1, 7, 7500 -2100 ); //K0R ics_set_pos ( UART_SIO3, 7, 7500 +2100 ); //K0L ics_set_pos ( UART_SIO1, 8, 9260 -4200 ); //HR ics_set_pos ( UART_SIO3, 8, 5740 +4200 ); //HL ics_set_pos ( UART_SIO1, 9, 7910 +2100 ); //A0R ics_set_pos ( UART_SIO3, 9, 7100 -2100 ); //A0L delay(30); ics_set_pos ( UART_SIO1, 5, 0 ); //K2R ics_set_pos ( UART_SIO2, 1, 0 ); //U0R ics_set_pos ( UART_SIO3, 5, 0 ); //K2L ics_set_pos ( UART_SIO4, 1, 0 ); //U0L ics_set_pos ( UART_SIO1, 6, 0 ); //K1R ics_set_pos ( UART_SIO2, 2, 0 ); //U1R ics_set_pos ( UART_SIO3, 6, 0 ); //K1L ics_set_pos ( UART_SIO4, 2, 0 ); //U1L ics_set_pos ( UART_SIO1, 7, 0 ); //K0R ics_set_pos ( UART_SIO3, 7, 0 ); //K0L ics_set_pos ( UART_SIO1, 8, 0 ); //HR ics_set_pos ( UART_SIO2, 4, 0 ); //ER ics_set_pos ( UART_SIO3, 8, 0 ); //HL ics_set_pos ( UART_SIO4, 4, 0 ); //EL ics_set_pos ( UART_SIO1, 9, 0 ); //A0R ics_set_pos ( UART_SIO3, 9, 0 ); //A0L sprintf( (char *)dsp,"turn over :%4d %4d\r\n",mode,pitchs ); printS ( (char *)dsp ); while(1){} } ///////////////////// //// UVC補助制御 //// ///////////////////// void uvcSub(void){ // ************ UVC終了時支持脚を垂直に戻す ************ if( fwct<=landF ){ // **** 左右方向 **** k = dyi/(11-fwct); dyi -= k; dyis += k; // **** 前後方向 **** if( dxi>RST_F_LEG ){ dxi -= RST_F_LEG; dxis -= RST_F_LEG; } else if( dxi<-RST_F_LEG ){ dxi += RST_F_LEG; dxis += RST_F_LEG; } else{ dxis -= dxi; dxi = 0; } } if(dyis> 70) dyis= 70; if(dxis<-70) dxis= -70; if(dxis> 70) dxis= 70; // ************ 脚長制御 ************ if(HEIGHT>autoH){ //脚長を徐々に復帰させる autoH +=(HEIGHT-autoH)*0.07;//0.07 } else autoH = HEIGHT; if( fwct>fwctEnd-landB && rollt>0){ //着地時姿勢を落し衝撃吸収 autoH -= ( fabs(dyi-dyib)+fabs(dxi-dxib) ) * 0.02; } if(140>autoH)autoH=140; //最低脚長補償 } void uvcSub2(void){ float k0,k1; // ************ UVC終了時支持脚を垂直に戻す ************ // **** 左右方向 **** k1 = dyi/(fwctEnd-fwct+1); dyi -= k1; // **** 前後方向 **** k0 = dxi/(fwctEnd-fwct+1); dxi -= k0; // **** 腰回転 **** wk -= wk/(fwctEnd-fwct+1); WESTW -= WESTW/(fwctEnd-fwct+1); K2W[0] = WESTW; K2W[1] =-WESTW; if( fwct<=landF ){ // **** 左右方向 **** dyis += k1; // **** 前後方向 **** dxis -= k0; } else{ dyis -= dyis/(fwctEnd-fwct+1); dxis -= dxis/(fwctEnd-fwct+1); } // **** 脚長制御 **** autoH += (HEIGHT-autoH)/(fwctEnd-fwct+1); if(dyis> 70) dyis= 70; if(dxis<-70) dxis= -70; if(dxis> 70) dxis= 70; } ///////////////// //// UVC制御 //// ///////////////// void uvc(void){ float pb,rb,k; // ************ 傾斜角へのオフセット適用 ************ rb=roll; //一時退避 pb=pitch; k=sqrt(pitch*pitch+roll*roll); //合成傾斜角 if( k>0.033 ){ k=(k-0.033)/k; pitch *=k; roll *=k; } else{ pitch =0; roll =0; } // ************ 傾斜角に係数適用 ************ rollt =0.25*roll; if(jikuasi==0) rollt = -rollt; //横方向符号調整 pitcht=0.25*pitch; if(fwct>landF && fwct<=fwctEnd-landB ){ // ************ UVC主計算 ************ k = atan ((dyi-sw)/autoH ); //片脚の鉛直に対する開脚角 kl = autoH/cos(k); //前から見た脚投影長 ks = k+rollt; //開脚角に横傾き角を加算 k = kl*sin(ks); //中点から横接地点までの左右距離 dyi = k+sw; //横方向UVC補正距離 autoH = kl*cos(ks); //K1までの高さ更新 // **** UVC（前後） ***** k = atan( dxi/autoH ); //片脚のX駆動面鉛直から見た現時点の前後開脚角 kl = autoH/cos(k); //片脚�