📄 main.c
字号:
void HandshakeL(void){
if(shakeL==1){
if((sequence>=0)&&(sequence<100)){ swing19U=ON;}
else if(sequence>=100){ swing19U=OFF;}
}
else if(shakeL==2){
if((sequence>=0)&&(sequence<50)){ swing22D=OFF; swing22U=ON;}
else if((sequence>=50)&&(sequence<100)){ swing22U=OFF; swing22D=ON;}
else if(sequence>=100){ sequence=0;}
}
else if(shakeL==3){
swing19D=ON; swing22D=ON;
if(sequence>=100){init_param(); shakeL=0;}
}
}
/**************************************************
*
* Initial value of posture sensor: 2-axis.
*
****************************************************/
#define XCENTER 0x84
#define YCENTER 0x84
/************************************************************************
* ad(void)
* This function carry out A/D conversion and posture control
* using posture sensor, mainly to control pitch movement.
*
* ad1_buffer[] is a ring buffer.
*
**************************************************************************/
void ad(void){
int i,j;
while(!AD0.CSR.BIT.ADF && !AD1.CSR.BIT.ADF);
AD0.CSR.BIT.ADF=0;
AD1.CSR.BIT.ADF=0;
ad1_buffer[buffer]=AD0.DRA.BYTE;
ad2_buffer[buffer]=AD1.DRA.BYTE;
AD0.CSR.BIT.ADST=1; /*AD start*/
AD1.CSR.BIT.ADST=1;
/****************************************
* Noise filter:
* heuristically, posture sensor output
* only vary about -10 < X < +10
* thus, any value that exceed this range
* is considered as noise, and cut off.
*****************************************
if(ad1_buffer[buffer]>=XCENTER+10) ad1_buffer[buffer]=XCENTER;
else if(ad1_buffer[buffer]<=XCENTER-10) ad1_buffer[buffer]=XCENTER;
if(ad2_buffer[buffer]>=YCENTER+20) ad2_buffer[buffer]=YCENTER;
else if(ad2_buffer[buffer]<=YCENTER-20) ad2_buffer[buffer]=YCENTER;
/*****************************************
* This part computes moving average
*******************************************/
ad0=ad1=0;
for(i=0;i<AVERAGE;i++){
j=(buffer-i)&BUFFER_SIZE;
ad1+=ad1_buffer[j];
ad2+=ad2_buffer[j];
}
ad1=ad1/AVERAGE;
ad2=ad2/AVERAGE;
/********************************************
* Counter for a ring buffer
*********************************************/
buffer++;
if(buffer==BUFFER_SIZE) buffer=0;
/*********************************************
* This part carry out proportional control of
* posture by comparing values of ad1, ad2 with
* XCENTER, YCENTER. Control is imposed on pitch
* pitch axis of ancle joints.
***********************************************/
if(ad1<=0x0A0){
if((ad1-XCENTER)>0x04) {output[1]+=(ad1-XCENTER)/2;output[7]-=(ad1-XCENTER)/2;}
else if((XCENTER-ad1)>=0x04) {output[1]-=(XCENTER-ad1)/2;output[7]+=(XCENTER-ad1)/2;}
}
}
/*************************************************************
*
* Walking pattern section
*
***************************************************************/
#define SIDE_LIMIT_R 0x0015
#define SIDE_LIMIT_L 0x0015
/**************************************************
*
* Forward walk pattern generation
*
****************************************************/
void walkforward(void){
int i;
for(i=0;i<12;i++){
output[i] = Pulse[i]-ch[i][counter];
if((i==4)&&(output[i]>Pulse[i]+SIDE_LIMIT_R)) output[i]=Pulse[i]+SIDE_LIMIT_R;
else if((i==4)&&(output[i]<Pulse[i]-SIDE_LIMIT_R)) output[i]=Pulse[i]-SIDE_LIMIT_R;
if((i==10)&&(output[i]>Pulse[i]+SIDE_LIMIT_L)) output[i]=Pulse[i]+SIDE_LIMIT_L;
else if((i==10)&&(output[i]<Pulse[i]-SIDE_LIMIT_L)) output[i]=Pulse[i]-SIDE_LIMIT_L;
}
for(i=12;i<26;i++){
output[i] = Pulse[i];
}
output[4]-=5;
output[10]+=5;
/*慜恑*/
if(walkF==ON){
output[1]+=4;
output[7]-=4;
}
/*屻恑*/
else if(walkB==ON){
output[1]+=3;
output[7]-=3;
}
/*塃慁夞*/
else if(walkR==ON){
output[1]+=3;
output[7]-=3;
output[5]=Pulse[5]-movedata[3][counter];
output[11]=Pulse[11]+movedata[3][counter];
}
/*嵍慁夞*/
else if(walkL==ON){
output[1]+=3;
output[7]-=3;
output[5]=Pulse[5]+movedata[3][counter];
output[11]=Pulse[11]-movedata[3][counter];
}
/* 榬傪怳傞*/
output[14]=Pulse[14]+movedata[0][counter];
output[19]=Pulse[19]+movedata[0][counter];
output[17]=Pulse[17]-movedata[1][counter];
output[22]=Pulse[22]-movedata[2][counter];
/*奺儌乕僔儑儞廔椆*/
if(walkF==2){
walkF=OFF;
initialdata();
}
if(walkB==2){
walkB=OFF;
initialdata();
}
if(walkR==2){
walkR=OFF;
initialdata();
}
if(walkL==2){
walkL=OFF;
initialdata();
}
if(counter>=DATALENGTH) counter=0;
counter++;
}
/**************************************************
* receive_data
* Recieve command from console input, and assign behavior flags.
*
****************************************************/
void receive_data(void){
char data;
if(SCI2.SC1SSR.WORD & 0x0040){ /* 庴怣僨乕僞偑偁傞偐丠 */
data=s_getc();
switch(data){
/*弶婜巔惃*/
case '0': init_param();initialposition();s_puts("Init\r\n"); break;
case '2': if(AD_FLAG==ON){AD_FLAG=OFF;s_puts("AD OFF\r\n");}
else{AD_FLAG=ON;s_puts("AD ON\r\n");}
break;
/*掆巭*/
case 'z': init_param(); s_puts("Stop\r\n"); break;
/*庱 嵍塃怳傝(ch25)*/
case 'a': init_param(); swing25R=ON; s_puts("swing25R\r\n"); break;
case 'b': init_param(); swing25C=ON; s_puts("swing25C\r\n"); break;
case 'c': init_param(); swing25L=ON; s_puts("swing25L\r\n"); break;
/*庱 忋壓怳傝(ch24)*/
case 'd': init_param(); swing24U=ON; s_puts("swing24U\r\n"); break;
case 'e': init_param(); swing24C=ON; s_puts("swing24C\r\n"); break;
case 'f': init_param(); swing24D=ON; s_puts("swing24D\r\n"); break;
/*塃旾 忋壓怳傝(ch17)*/
case 'g': init_param(); swing17U=ON; s_puts("swing17U\r\n"); break;
case 'h': init_param(); swing17C=ON; s_puts("swing17C\r\n"); break;
case 'i': init_param(); swing17D=ON; s_puts("swing17D\r\n"); break;
/*塃尐 忋壓怳傝(ch14)*/
case 'j': init_param(); swing14U=ON; s_puts("swing14U\r\n"); break;
case 'k': init_param(); swing14C=ON; s_puts("swing14C\r\n"); break;
case 'l': init_param(); swing14D=ON; s_puts("swing14D\r\n"); break;
/*嵍旾 忋壓怳傝(ch22)*/
case 'm':init_param(); swing22U=ON; s_puts("swing22U\r\n"); break;
case 'n': init_param(); swing22C=ON; s_puts("swing22C\r\n"); break;
case 'o': init_param(); swing22D=ON; s_puts("swing22D\r\n"); break;
/*嵍尐 忋壓怳傝(ch19)*/
case 'p': init_param(); swing19U=ON; s_puts("swing19U\r\n"); break;
case 'q': init_param(); swing19C=ON; s_puts("swing19C\r\n"); break;
case 'r': init_param(); swing19D=ON; s_puts("swing19D\r\n"); break;
/*塃尐 嵍塃忋壓怳傝(ch15)*/
case 's': init_param(); swing15U=ON; s_puts("swing15U\r\n"); break;
case 't': init_param(); swing15C=ON; s_puts("swing15C\r\n"); break;
case 'u': init_param(); swing15D=ON; s_puts("swing15D\r\n"); break;
/*嵍尐 嵍塃忋壓怳傝(ch20)*/
case 'v': init_param(); swing20U=ON; s_puts("swing20U\r\n"); break;
case 'w': init_param(); swing20C=ON; s_puts("swing20C\r\n"); break;
case 'x': init_param(); swing20D=ON; s_puts("swing20D\r\n"); break;
/*摲懱忋 嵍塃怳傝(ch13)*/
case 'A': init_param(); swing13R=ON; s_puts("swing13R\r\n"); break;
case 'B': init_param(); swing13C=ON; s_puts("swing13C\r\n"); break;
case 'C': init_param(); swing13L=ON; s_puts("swing13L\r\n"); break;
case 'Z': init_param(); s_puts("caliberation"); cab(); break;
/*暋崌塣摦*/
/*庤傪嫇偘傞*/
case 'D': init_param(); lupR=ON; s_puts("Liftup R\r\n");
sequence=0; break;
case 'E': init_param(); ldownR=ON; s_puts("Liftdouw R\r\n");
sequence=0; break;
case 'F': init_param(); lupL=ON; s_puts("Liftup L\r\n");
sequence=0; break;
case 'G': init_param(); ldownL=ON; s_puts("Liftdown L\r\n");
sequence=0; break;
case 'H': init_param(); lupR=lupL=ON; s_puts("Liftup RL\r\n");
sequence=0; break;
case 'I': init_param(); ldownR=ldownL=ON; s_puts("Liftdown RL\r\n");
sequence=0; break;
/*僶僀僶僀*/
case '9': init_param(); byeR=ON; s_puts("Bye ByeR\r\n");
sequence=0; break;
case '8': init_param(); byeL=ON; s_puts("Bye ByeL\r\n");
sequence=0; break;
case '7': init_param(); byeR=byeL=ON; s_puts("Bye Bye Bye Bye\r\n");
sequence=0; break;
/*埇庤*/
case '6': init_param(); shakeR++; s_puts("Hand shakeR\r\n");
sequence=0; break;
case '5': init_param(); shakeL++; s_puts("Hand shakeL\r\n");
sequence=0; break;
/*曕峴*/
case '1': init_param(); walkF++; s_puts("Walk Forward\r\n");
counter=0; break;
/*屻傠曕峴*/
case '3': init_param(); walkB++; s_puts("Walk Backward\r\ns");
counter=0; break;
/*塃慁夞*/
case 'R': init_param(); walkR++; s_puts("Walk Backward\r\n");
counter=0; break;
/*嵍慁夞*/
case 'L': init_param(); walkL++; s_puts("Walk Backward\r\n");
counter=0; break;
default: break;
}
}
}
/**************************************************
* standing
* set initial value as defined in Pulse[i] to output[i]
*
****************************************************/
void standing(void){
int i;
for(i=0;i<12;i++){
output[i] = Pulse[i];
}
}
/**************************************************
* senddata transmit output[i] to SH2 via serial port.
*
*
****************************************************/
void senddata(void){
int i;
/*Send Position data*/
for(i=0;i<26;i++){
PB.DR.WORD.L = (output[i]&0x1000)>>9;
PE.DR.WORD.L = (output[i]&0x000f)<<12;
PE.DR.WORD.H = (output[i]&0x0ff0)>>4;
//PEDR=output[i]; 7050
//PB.DR.WORD.H&=~((0x1f-i)&0x1f); /*set the address*/
PB.DR.WORD.H&=i; /*set the address*/
backup[i]=output[i];
wait(0xff);
PB.DR.WORD.H|=0x1f; /*clear the address*/
PB.DR.WORD.L|=0x0040; /*clear the data*/
PE.DR.WORD.L|=0xf000;
PE.DR.WORD.H|=0x00ff;
}
PB.DR.WORD.L =0x0040; /*clear the data*/
PE.DR.WORD.L =0xf000;
PE.DR.WORD.H =0x00ff;
}
/**************************************************
*
* Timer initializatoin
*
****************************************************/
void TPU0_timer_init(void){
TPU.TSTR.BYTE &= (~0x01); /* TCNT0 僇僂儞僩掆巭 */
TPU0.TCR.BYTE = 0x22; /* 僐儞儁傾儅僢僠偱TCNT僋儕傾 */
/* 棫偪忋偑傝僄僢僕偱僇僂儞僩 */
/* 僋儘僢僋 P/16 */
TPU0.TMDR.BYTE = 0x00; /* TGRA,TGRB 捠忢摦嶌 */
TPU0.TIOR.WORD = 0x0000; /* I/O弌椡嬛巭 */
TPU0.TCNT = 0x0000; /* 僇僂儞僞僋儕傾 */
TPU0.TGRA = ((14*30*1000)/16)-1; /* 僞僀儅 14msec */
TPU0.TIER.BYTE = 0x01; /* TGFA妱崬傒嫋壜 */
TPU0.TSR.BYTE &= 0x00; /* 僗僥乕僞僗僋儕傾 */
}
/**************************************************
*
* Initialize interrupt levels
*
****************************************************/
void int_init(void){
/*桪愭儗儀儖愝掕 16 <- 0 0=MASK */
INTC.IPRH.WORD = 0x7000; /* TPU0 Level7 */
INTC.IPRK.WORD = 0x0070; /* SCI2 Level7 */
INTC.IPRL.WORD = 0x7000; /* CMT Level7 */
}
/********************************************************************
* timer (interrupt procedure)
* every 13.5 ms, timer(void) is invoked as an interrupt
* call.
*
* It checks body balance using ad(), then goes through
* all behavior commands to check behavior flag status,
* and set appropriate output[i] value.
* Then, it sends output[i] using senddata().
*
*********************************************************************/
#pragma interrupt
void timer(void){
TPU0.TCNT = 0x0000;
TPU0.TSR.BYTE &= (~0x01);
if((AD_FLAG==ON)&&(walkF==OFF)){
standing();
ad();
}
if((AD_FLAG==ON)&&(walkF!=OFF)){
ad();
}
swing_ch13();
swing_ch14();
swing_ch15();
swing_ch17();
swing_ch19();
swing_ch20();
swing_ch22();
swing_ch24();
swing_ch25();
if(byeR==ON) byebyeR();
if(byeL==ON) byebyeL();
if(lupR==ON) liftupR();
if(ldownR==ON) liftdownR();
if(lupL==ON) liftupL();
if(ldownL==ON) liftdownL();
if(shakeR!=OFF) HandshakeR();
if(shakeL!=OFF) HandshakeL();
if(walkF!=OFF) walkforward();
if(walkF!=OFF) walkforward();
if(walkB!=OFF) walkforward();
if(walkR!=OFF) walkforward();
if(walkL!=OFF) walkforward();
senddata();
sequence++;
adcounter++;
}
/**************************************************************
*
* main function
*
****************************************************************/
int main(void){
int i;
PEIOR=0x7fff; /*for DATA BUS set OUTPUT*/
PFIOR=0x3f; /*for ADDRESS BUS set OUTPUT*/
ADCSR0=0x23; /* 8ch scan mode, no interrupt 0 0 1 0 0 0 1 1*/
iniSCI2(38400);
int_init();
setvect(140)=(unsigned long )timer;
set_imask( 5 ); /* 妱傝崬傒儅僗僋愝掕 */
TPU0_timer_init();
init_param();
shakeR=shakeL=OFF;
walkF=walkB=walkR=walkL=OFF;
initialdata();
AD0.CSR.BIT.ADST=1; /*AD start*/
AD1.CSR.BIT.ADST=1;
PB.DR.WORD.L =0x0001; /*clear the data*/
PE.DR.WORD.L =0xf000;
PE.DR.WORD.H =0x00ff;
senddata();
s_puts("Init\r\n");
TPU.TSTR.BYTE |= 0x01; /* TCNT0 僇僂儞僩奐巒 */
while(1){
receive_data();
}
}
/*************************************************************************
* cab
*
*
*
**************************************************************************/
int cab(void){
unsigned int tmp,tmpch,i;
while(1){
tmp=s_getc();
if((tmp-'a'<26)&&(tmp-'a'>=0)){
tmpch=(unsigned int)(tmp-'a');
s_puts("\r\n");s_putDec(tmp-'a');s_puts("\r\n");
}
else if(tmp=='+'){Pulse[tmpch]+=1;s_puts("+\r\n");}
else if(tmp=='-'){Pulse[tmpch]-=1;s_puts("-\r\n");}
else if(tmp=='P'){
for(i=0;i<25;i++){
s_puts("0x");s_putWord(Pulse[i]);s_puts(",\r\n");
}
s_puts("0x");s_putWord(Pulse[25]);s_puts("\r\n");
}
else if(tmp=='Z'){
s_puts("caribration off\r\n");
return 0;
}
initialdata();
}
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -