main.c

东芝机器人处理器代码,sh2有时间学习很好！

/*********************************************************************
 *  PHR-001 (PINO-class Humanoid Robot 001) Behavior Command Program
 *  Main program
 *
 *       File Created: 28, Mar. 2001 by  F.Yamasaki
 *       Modified: 28, Jun. 2001     by  K.Endo
 *       Annotations: 15, Oct. 2001  by  F. Yamasaki, K. Endo, H. Kitano
 *
 *  Copyright (C) 2001 Kitano Symbiotic Systems Project, 
 *  Japan Science and Technology Corp.
 *  
 *  This file is part of PHR-001.
 *  
 *  PHR-001 is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *  
 *  PHR-001 is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *  
 *  You should have received a copy of the GNU General Public License
 *  along with PHR-001; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *********************************************************************/
/**********************************************************************
*
* This program mainly consists of main() function
* and timer() interrupt function.
* Within "main()" function, recieve_data() takes concole input, and
* set behavior flags.
* Every 13.5 ms, timer() interupt, and checks all flags and
* execute corresponding behavior commands, set values of output[],
* then, senddata transfer output[] via serial port.
*
************************************************************************/

#include "sh7060s.h"
#include "Def.h"
#include "serial.h"
#include "sub.h"
#include "header.h"

/************************************************************
* CHNUM and DATALENGTH defines matrix size of
* data points for pre-computed walking patterns.
*
*************************************************************/
#define	CHNUM		12  /* Num. actuators used in walk pattern */
#define	DATALENGTH	150 /* Num. of data points for one cycle */

/***********************************
* function prototype definitions
************************************/

uchar s_getc(void);
void s_putc(schar);
void s_putWord(uint);
int cab(void);
void __main(){
}
void exit(){
	for(;;);
}


/*******************************************************************
*
* Pulse[26] defines initial values that are send to servo motors.
* In this system, one cycle is 13.5 ms. Signal pattern is defined
* using 4320 units within each 13.5 ms cycle.
*
*           ____                           ____
*          |    |                         |    |
*          |    |                         |    |
*          |    |                         |    |
*  ________|    |_________________________|    |___
*
*           <--- 13.5[ms](4320count) ---->
*
* Value of Pulse[i] represents angle of each servo motor for
* initial positition: standing.
*
*********************************************************************/

unsigned int Pulse[26]={
0x0189,
0x0162,
0x0171,
0x0170,
0x0163,
0x0146,
0x0191,
0x01A9,
0x01A9,
0x0177,
0x0169,
0x017F,
0x015E,
0x017F,
0x0068,
0x0229,
0x017C,
0x0175,
0x0146,
0x0286,
0x00B4,
0x0152,
0x019B,
0x016B,
0x0136,
0x0169
};

/*******************************************************************
*
* ch[][] defines time series data points for pre-computed 
* walking pattern.
* Target joint angle "output[i]" is created by adding initial
* angle "Pulse[i]" and "ch[i][time]" at each time point:
*   output[i] = Pulse[i]+ch[i][time]
*
* ch number and joint corrensondence is shown in a separate figure.
* ch0 - ch5: right leg.
* ch6 - ch11: left leg.
* ch12, 13:  torso
* ch14 - ch18: right arm
* ch19 - ch23: left arm
* ch24, 25:  neck
*
* For walking behavior, only channel 0 - 11 are used.
********************************************************************/

int ch[CHNUM][DATALENGTH]={{
#include "ch0.txt"
},{
#include "ch1.txt"
},{
#include "ch2.txt"
},{
#include "ch3.txt"
},{
#include "ch4.txt"
},{
#include "ch5.txt"
},{
#include "ch6.txt"
},{
#include "ch7.txt"
},{
#include "ch8.txt"
},{
#include "ch9.txt"
},{
#include "ch10.txt"
},{
#include "ch11.txt"
}};

/*******************************************************************
* movedata
* time series data for torso and arm movement during walk patterns.
*
*********************************************************************/

int movedata[4][DATALENGTH]={{
#include "move1.txt"
},{
#include "move2.txt"
},{
#include "move3.txt"
},{
#include "move4.txt"
}};

/*******************************************************************
*
* Variable definitions
*
********************************************************************/

int backup[26]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
int output[26]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};

int counter,sequence;

/*********************************************************************
*
* Behavior flags
*
***********************************************************************/

int walkF, walkB, walkR, walkL;
int byeR, byeL;
int lupR, ldownR, lupL, ldownL;
int shakeR, shakeL;
int swing13R, swing13C, swing13L;
int swing14U, swing14C, swing14D;
int swing15U, swing15C, swing15D;
int swing17U, swing17C, swing17D;
int swing19U, swing19C, swing19D;
int swing20U, swing20C, swing20D;
int swing22U, swing22C, swing22D;
int swing24U, swing24C, swing24D;
int swing25R, swing25C, swing25L;

int BUFFER_SIZE=0x0f;
int buffer=0;
int AVERAGE=13;
int adcounter=0;
int ad0,ad1;
int ad0_buffer[0x0f],ad1_buffer[0x0f];
int AD_FLAG=1;

/****************************************************************
* init_param()
* Initialize all behavior flags to OFF
*
******************************************************************/

void init_param(void){
  byeR=byeL=
  lupR=ldownR=lupL=ldownL=
  swing13R=swing13C=swing13L=
  swing14U=swing14C=swing14D=
  swing15U=swing15C=swing15D=
  swing17U=swing17C=swing17D=
  swing19U=swing19C=swing19D=
  swing20U=swing20C=swing20D=
  swing22U=swing22C=swing22D=
  swing24U=swing24C=swing24D=
  swing25R=swing25C=swing25L=OFF;
}

/***************************************************************
* initialdata()
* Initialize data by copying Pulse[i] to output[i]
*
*****************************************************************/

void initialdata(void){
  for(i=0;i<26;i++){
    output[i] = Pulse[i];
  }
}

/**************************************************************
* initialposition()
* Set behavioral flags to initial state
*
*****************************************************************/

void initialposition(void){
  swing13C = swing14C = swing15C =
  swing17C = swing19C = swing20C =
  swing22C = swing24C = swing25C = ON;
}

/**************************************************
*
* Torso motor swing
*
****************************************************/

void swing_ch13(void){
  if(swing13L==ON){if(output[13]>CH13MIN) output[13]-=1;}
  else if(swing13R==ON){if(output[13]<CH13MAX) output[13]+=1;}
  else if(swing13C==ON){
    if(output[13]>Pulse[13]+1){ output[13]-=1;}
    else if(output[13]<Pulse[13]-1){ output[13]+=1;}
    else{ swing13C=OFF;}
  }
}

/**************************************************
*
* Right shoulder motor up-down swing
*
****************************************************/

void swing_ch14(void){
  if(swing14D==ON){if(output[14]>CH14MIN) output[14]-=2;}
  else if(swing14U==ON){if(output[14]<CH14MAX) output[14]+=2;}
  else if(swing14C==ON){
    if(output[14]>Pulse[14]+2){ output[14]-=2;}
    else if(output[14]<Pulse[14]-2){ output[14]+=2;}
    else{ swing14C=OFF;}
  }
}

/**************************************************
*
* Right shoulder motor left-right swing
*
****************************************************/

void swing_ch15(void){
  if(swing15U==ON){if(output[15]>CH15MIN) output[15]-=2;}
  else if(swing15D==ON){if(output[15]<CH15MAX) output[15]+=2;}
  else if(swing15C==ON){
    if(output[15]>Pulse[15]+2){ output[15]-=2;}
    else if(output[15]<Pulse[15]-2){ output[15]+=2;}
    else{ swing15C=OFF;}
  }
}

/**************************************************
*
* Right arm up-down swing
*
****************************************************/

void swing_ch17(void){
  if(swing17U==ON){if(output[17]>CH17MIN) output[17]-=2;}
  else if(swing17D==ON){if(output[17]<CH17MAX) output[17]+=2;}
  else if(swing17C==ON){
    if(output[17]>Pulse[17]+2){ output[17]-=2;}
    else if(output[17]<Pulse[17]-2){ output[17]+=2;}
    else{ swing17C=OFF;}
  }
}

/**************************************************
*
* Left arm up-down swing
*
****************************************************/

void swing_ch19(void){
  if(swing19U==ON){if(output[19]>CH19MIN) output[19]-=2;}
  else if(swing19D==ON){if(output[19]<CH19MAX) output[19]+=2;}
  else if(swing19C==ON){
    if(output[19]>Pulse[19]+2){ output[19]-=2;}
    else if(output[19]<Pulse[19]-2){ output[19]+=2;}
    else{ swing19C=OFF;}
  }
}

/**************************************************
*
* Left shoulder left-right swing
*
****************************************************/

void swing_ch20(void){
  if(swing20D==ON){if(output[20]>CH20MIN) output[20]-=2;}
  else if(swing20U==ON){if(output[20]<CH20MAX) output[20]+=2;}
  else if(swing20C==ON){
    if(output[20]>Pulse[20]+2){ output[20]-=2;}
    else if(output[20]<Pulse[20]-2){ output[20]+=2;}
    else{ swing20C=OFF;}
  }
}

/**************************************************
*
* Left arm up-down swing
*
****************************************************/

void swing_ch22(void){
  if(swing22D==ON){if(output[22]>CH22MIN) output[22]-=2;}
  else if(swing22U==ON){if(output[22]<CH22MAX) output[22]+=2;}
  else if(swing22C==ON){
    if(output[22]>Pulse[22]+2){ output[22]-=2;}
    else if(output[22]<Pulse[22]-2){ output[22]+=2;}
    else{ swing22C=OFF;}
  }
}

/**************************************************
*
* Neck up-down swing
*
****************************************************/

void swing_ch24(void){
  if(swing24D==ON){if(output[24]>CH24MIN) output[24]-=2;}
  else if(swing24U==ON){if(output[24]<CH24MAX) output[24]+=2;}
  else if(swing24C==ON){
    if(output[24]>Pulse[24]+2){ output[24]-=2;}
    else if(output[24]<Pulse[24]-2){ output[24]+=2;}
    else{ swing24C=OFF;}
  }
}

/**************************************************
*
* Neck left-right rotate
*
****************************************************/

void swing_ch25(void){
  if(swing25R==ON){if(output[25]>CH25MIN) output[25]-=2;}
  else if(swing25L==ON){if(output[25]<CH25MAX) output[25]+=2;}
  else if(swing25C==ON){
    if(output[25]>Pulse[25]+2){ output[25]-=2;}
    else if(output[25]<Pulse[25]-2){ output[25]+=2;}
    else{ swing25C=OFF;}
  }
}

/************************************************************
*
* Complex behaviors
*
***************************************************************/

/**************************************************
* byebyeR
* wave right arm to do "byebye"
*
****************************************************/

void byebyeR(void){
  if((sequence>=0)&&(sequence<100)){	swing14U=ON;	swing15U=ON;}
  else if((sequence>=100)&&(sequence<150)){	swing14U=OFF;	swing15U=OFF;	swing17U=ON;}
  else if((sequence>=150)&&(sequence<200)){	swing17U=OFF;	swing17D=ON;}
  else if((sequence>=200)&&(sequence<250)){	swing17D=OFF;	swing17U=ON;}
  else if((sequence>=250)&&(sequence<300)){	swing17U=OFF;	swing17D=ON;}
  else if((sequence>=300)&&(sequence<350)){	swing17D=OFF;	swing17U=ON;}
  else if((sequence>=350)&&(sequence<400)){	swing17U=OFF;	swing17D=ON;}
  else if((sequence>=400)&&(sequence<600)){	swing17D=OFF;	swing14D=ON;	swing15D=ON;}
  else if(sequence>=600){init_param();	byeR=OFF;}
}

/**************************************************
* byebyeL
* wave left arm to do "byebye"
*
****************************************************/

void byebyeL(void){
  if((sequence>=0)&&(sequence<100)){	swing19U=ON;	swing20U=ON;}
  else if((sequence>=100)&&(sequence<150)){	swing19U=OFF;	swing20U=OFF;	swing22U=ON;}
  else if((sequence>=150)&&(sequence<200)){	swing22U=OFF;	swing22D=ON;}
  else if((sequence>=200)&&(sequence<250)){	swing22D=OFF;	swing22U=ON;}
  else if((sequence>=250)&&(sequence<300)){	swing22U=OFF;	swing22D=ON;}
  else if((sequence>=300)&&(sequence<350)){	swing22D=OFF;	swing22U=ON;}
  else if((sequence>=350)&&(sequence<400)){	swing22U=OFF;	swing22D=ON;}
  else if((sequence>=400)&&(sequence<600)){	swing22D=OFF;	swing19D=ON;	swing20D=ON;}
  else if(sequence>=600){init_param();	byeR=OFF;}
}

/**************************************************
*
*
*
****************************************************/

void liftupR(void){
  if((sequence>=0)&&(sequence<100)){	swing17U=ON;}
  else if((sequence>=100)&&(sequence<600)){	swing14U=ON;}
  else if(sequence>=600){init_param();	lupR=OFF;}
}

/**************************************************
*
*
*
****************************************************/

void liftupL(void){
  if((sequence>=0)&&(sequence<100)){	swing22U=ON;}
  else if((sequence>=100)&&(sequence<300)){	swing19U=ON;}
  else if(sequence>=300){init_param();	lupL=OFF;}
}

/**************************************************
*
*
*
****************************************************/

void liftdownR(void){
  swing17D=ON;	swing14D=ON;
  if(sequence>=200){init_param();	ldownR=OFF;}
}

/**************************************************
*
*
*
****************************************************/

void liftdownL(void){
  swing22D=ON;	swing19D=ON;
  if(sequence>=200){init_param();	ldownL=OFF;}
}

/**************************************************
* handshake using right arm
*
*
****************************************************/

void HandshakeR(void){
  if(shakeR==1){
    if((sequence>=0)&&(sequence<100)){ swing14U=ON;}
    else if(sequence>=100){ swing14U=OFF;}
  }
  else if(shakeR==2){
    if((sequence>=0)&&(sequence<50)){ swing17D=OFF; swing17U=ON;}
    else if((sequence>=50)&&(sequence<100)){ swing17U=OFF; swing17D=ON;}
    else if(sequence>=100){ sequence=0;}
  }
  else if(shakeR==3){
    swing14D=ON;	swing17D=ON;
    if(sequence>=100){init_param();	shakeR=0;}
  }
}

/**************************************************
* handshake using left arm
*
*
****************************************************/