#include "MIIIRobot.h" //数学函数，返回两个数的较小的值 int min(int a, int b) { if (a < b) return a; else return b; } //数学函数，返回两个数的较大的值 int max(int x, int y) { if (x > y) return x; else return y; } //让CPU等待seconds的时间，seconds单位为秒 void sleep(float seconds) { wait(seconds); } //软件复位，目前没有作用，留作以后版本更新 void asosreset() { } //读模拟口，channel=0~7,返回:0-1023.当拨码开关设定为on,0号口为右光敏，1为左光敏，2为麦克风，3为碰撞环，4为红外接收，5,6,7为BUS总线占用 int analogport(int channel) { return AI(channel); } //光敏探测器的读数，index=左1,右2，返回值0~1023，光线越暗数值越大 int photo(int index) { if (index==1) return AI(1); if (index==2) return AI(0); return 0; } //读运行键 返回值：释放0，按下1 int runbutton() { return Get_Button(); } //延时，msec单位毫秒 void msleep(long msec) { float sec = 0.0; sec=msec/1000; wait(sec); } //发一声 void beep() { tone(1000., .3); } //设置电机功率，左0,右1,功率设定-100~100 void motor(int index, int vel) { SetMoto(index-1,vel); } //控制能力风暴的平移和旋转。trans_vel平移速度，rot_vel旋转速度 void drive(int trans_vel, int rot_vel) { motor(1,trans_vel + rot_vel); motor(2,trans_vel - rot_vel); } //停止电机，index = 左0,右1 void stop_motor(int index) { SetMoto(index,0); } //停止两个主动轮电机 void stop() { SetMoto(0,0); SetMoto(1,0); } //设置电机功率，左0,右1,功率-100~100 void motor_dummy(int index, int vel) { SetMoto(index,vel); } //初始化串口通讯，串口使用默认参数(9600-8-1-n) void hold_serial() { SCI_Set(0,9600,0,8,1); } //初始化串口通讯，串口使用默认参数(9600-8-1-n)，保留此函数与MII系统兼容 void share_serial() { SCI_Set(0,9600,0,8,1); } //向串口多次发送一个字符，idata:要发送的数据，inum :发送次数， itime:数据发送时间间隔(单位为秒)，注：字符发送成功后函数才返回 void repeat_putchar(int idata,int inum,int itime) { if(inum > 0) { while(1) { serial_putchar(idata); wait((float)itime/1000.0); inum=inum-1; if(inum<=0){break;} } } } //读麦克风，返回值0~1023，声音越响数值越大,麦克风占用2号模拟口 int microphone() { int Maxval=0; int Minval=1023; int i=0; int MicPort = 2;//麦克风占用2号模拟口 while(1) { if(AI(MicPort)>Maxval){Maxval=AI(MicPort);} wait(0.001); if(AI(MicPort)5){break;} i++; } return (Maxval-Minval); } //读碰撞，返回值碰撞开关信号：无0,左前1,右前2,左后4,右后8,（前3,后12,左5,右10,） int bumper() { int bmpr; int BumpPort=3; bmpr = AI(BumpPort); /*return AI(BumpPort);*/ if (bmpr <= 8 ) return 0; /* */ if (bmpr <= 55) return 4; /* BLEFT */ if (bmpr <= 110) return 8; /* BRIGHT */ if (bmpr <= 165) return 12; /* BLEFT BRIGHT */ if (bmpr <= 200) return 1; /* FLEFT */ if (bmpr <= 250) return 5; /* BLEFT FLEFT */ /*if (bmpr <= 285) return 13; /* BLEFT BRIGHT FLEFT */ if (bmpr <= 375) return 2; /* FRIGHT*/ if (bmpr <= 385) return 10; /* BRIGHT FRIGHT*/ if (bmpr <= 400)return 3; /* FLEFT FRIGHT*/ /*if (bmpr <= 380) return 14; /* BLEFT BRIGHT FRIGHT*/ /*if (bmpr <= 420)return 11; /* BRIGHT FLEFT FRIGHT*/ /*if (bmpr <= 500) return 15; /* BLEFT BRIGHT FLEFT FRIGHT*/ return 0; } //数字输入端口，Channel为通道号，0,7为光电编码器输入，1,2为bus总线上的PA1,PA2端口,其他端口保留升级用 int digitalport(int Channel) { return DI(Channel); } //读取BUS总线上地址为loc的扩展卡返回的一个字节的数据,读取BUS总线上地址为loc的扩展卡的一个字节的返回数据，loc为0x4000(IS0),0x5000(IS1),0x6000(IS2),0x7000(IS3) int read(int loc) { return peek(loc); } //向BUS总线上地址为loc的扩展卡的写一个字节的数据，loc为0x4000(OS0),0x5000(OS1),0x6000(OS2),0x7000(OS3) void write(int loc, int byte) { poke(loc, byte); } //根据iCh(0,1,2,3)通道号,返回bus总线地址(0x4000,0x5000,0x6000,0x7000) int GetAddrFromCh(int iCh) { int iAddr = 0x4000; if(iCh == 0) iAddr = 0x4000; else if(iCh == 1) iAddr = 0x5000; else if(iCh == 2) iAddr = 0x6000; else if(iCh == 3) iAddr = 0x7000; else iAddr = 0x4000; return iAddr; } //设置高级伺服电机驱动卡的单个电机通道脉宽改变速度,iAddr为驱动卡地址：0x4000(OS0),0x5000(OS1),0x6000(OS2),0x7000(OS3),ch为伺服电机编号(0-7),speed为速度：（1-100,默认为20） int SetupServoSpeedCh(int iAddr,int ch,int speed) { int i =0; poke(iAddr,0xFF); i++; poke(iAddr,0x02); i++; poke(iAddr,ch); i++; poke(iAddr,speed); i++; poke(iAddr,0xFE); i++; return 0; } //设置高级伺服电机驱动卡的单个电机通道输出的脉宽（角度）,iAddr为驱动卡地址：0x4000(OS0),0x5000(OS1),0x6000(OS2),0x7000(OS3),ch为伺服电机编号(0-7),angle为角度：（0-180） int SetupServoAngleCh(int iAddr,int ch,int angle) { /*设置单个角度 格式：0xFF，0x01， 伺服电机号， 角度(0-180)， 0xFE 设置角度先保存,但是不改变输出脉宽,默认为0度*/ int i = 0; poke(iAddr,0xFF); i++; poke(iAddr,0x01); i++; poke(iAddr,ch); i++; poke(iAddr,angle); i++; poke(iAddr,0xFE); i++; return 0; } //设置高级伺服电机驱动卡的数字输出导通，ch为驱动卡地址0,1,2,3．chval为DO口输出状态（二进制：00000xxx）:最后三位对应输出编号,1为导通,0为保持当前状态 int SetupOutputOpenCh(int ch,int chval) { /*数字输出口开 0xFF 0x07 0x00 00000xxx 0xFE 210 对应输出编号,1为导通,0为保持当前状态*/ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x07); i++; poke(iAddr,0x00); i++; poke(iAddr,chval); i++; poke(iAddr,0xFE); i++; return 0; } //设置高级伺服电机驱动卡的数字输出关断，ch为驱动卡地址0,1,2,3．chval为DO口输出状态（二进制：00000xxx）:最后三位对应输出编号,1为关断,0为保持当前状态 int SetupOutputCloseCh(int ch,int chval) { /*数字输出口关 0xFF 0x08 0x00 00000xxx 0xFE 210 对应输出编号,1为关断,0为保持当前状态*/ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x08); i++; poke(iAddr,0x00); i++; poke(iAddr,chval); i++; poke(iAddr,0xFE); i++; return 0; } //设置高级伺服电机驱动卡的电机通道脉宽改变速度，ch为驱动卡地址0,1,2,3．iMask为对应掩码设为0xff，speed0-7为速度：（1-100,默认为20） void SVA_Speed(int ch,int iMask,int speed0,int speed1,int speed2,int speed3, int speed4,int speed5,int speed6,int speed7) { int iAddr = GetAddrFromCh(ch); if ((iMask & 0x1) != 0) SetupServoSpeedCh(iAddr,0,speed0); if ((iMask & 0x2) != 0) SetupServoSpeedCh(iAddr,1,speed1); if ((iMask & 0x4) != 0) SetupServoSpeedCh(iAddr,2,speed2); if ((iMask & 0x8) != 0) SetupServoSpeedCh(iAddr,3,speed3); if ((iMask & 0x10) != 0) SetupServoSpeedCh(iAddr,4,speed4); if ((iMask & 0x20) != 0) SetupServoSpeedCh(iAddr,5,speed5); if ((iMask & 0x40) != 0) SetupServoSpeedCh(iAddr,6,speed6); if ((iMask & 0x80) != 0) SetupServoSpeedCh(iAddr,7,speed7); } //设置高级伺服电机驱动卡的电机通道角度输出，ch为驱动卡地址0,1,2,3．iMask为对应掩码设为0xff，angle0-7为角度：（-90~90） void SVA_Pos(int ch,int iMask,int angle0,int angle1,int angle2,int angle3, int angle4,int angle5,int angle6,int angle7) { int iAddr = GetAddrFromCh(ch); if ((iMask & 0x1) != 0) SetupServoAngleCh(iAddr,0,angle0+90); if ((iMask & 0x2) != 0) SetupServoAngleCh(iAddr,1,angle1+90); if ((iMask & 0x4) != 0) SetupServoAngleCh(iAddr,2,angle2+90); if ((iMask & 0x8) != 0) SetupServoAngleCh(iAddr,3,angle3+90); if ((iMask & 0x10) != 0) SetupServoAngleCh(iAddr,4,angle4+90); if ((iMask & 0x20) != 0) SetupServoAngleCh(iAddr,5,angle5+90); if ((iMask & 0x40) != 0) SetupServoAngleCh(iAddr,6,angle6+90); if ((iMask & 0x80) != 0) SetupServoAngleCh(iAddr,7,angle7+90); } //设置高级伺服电机驱动卡的保护恢复时间,ch为驱动卡地址0,1,2,3,time为时间（0-100）秒,影响堵转指示灯,默认为5秒 void SVA_Ptc(int ch,int time) { /*设置保护恢复时间 格式： 0xFF 0x03 0x00 时间(0-100)秒 0xFE 影响堵转指示灯,默认为5秒*/ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x03); i++; poke(iAddr,0x00); i++; poke(iAddr,time); i++; poke(iAddr,0xFE); i++; } //设置高级伺服电机驱动卡的电机脉宽参数,ch为驱动卡地址0,1,2,3,PWM_TPmin为脉宽最小值单位毫秒（默认0.5毫秒），PWM_TPmax为脉宽最大值单位毫秒（默认2.5毫秒） void SVA_Pulse(int ch,float PWM_TPmin,float PWM_TPmax) { /*设置电机脉宽参数 0xFF 0x06 低(>=50) 高(<=250) 0xFE 对应0.5ms-2.5ms,适应不同舵机需要*/ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x06); i++; poke(iAddr,(int)(PWM_TPmin*100.0)); i++; poke(iAddr,(int)(PWM_TPmax*100.0)); i++; poke(iAddr,0xFE); i++; } //设置高级伺服电机驱动卡的电机开始转动到指定角度,ch为驱动卡地址0,1,2,3,iMask为对应掩码设为0xff void SVA_GO(int ch,int iMask) { /*开始转动到指定角度 0xFF 0x04 0000xxxx(chval1) 0000xxxx(chval2) 0xFE 指定角度通过"设置单个角度"设定 7654 3210 对应电机编号,1为转动,0为保持当前脉宽 */ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x04); i++; poke(iAddr,(iMask & 0xF0)/16); i++; poke(iAddr,(iMask & 0x0F)); i++; poke(iAddr,0xFE); i++; } //禁用高级伺服电机驱动卡,ch为驱动卡地址0,1,2,3.iMask为对应掩码设为0xff void SVA_Disable(int ch,int iMask) { /*禁用 0xFF 0x05 0000xxxx(chval1) 0000xxxx(chval2) 0xFE 对应电机编号,1为停止脉宽输出,0为保持当前脉宽 7654 3210 */ int iAddr; int i =0; iAddr = GetAddrFromCh(ch); poke(iAddr,0xFF); i++; poke(iAddr,0x05); i++; poke(iAddr,(iMask & 0xF0)/16); i++; poke(iAddr,(iMask & 0x0F)); i++; poke(iAddr,0xFE); i++; } //设置高级伺服电机驱动卡数字输出通道,ch为驱动卡地址0,1,2,3.iMask为对应掩码设为0x1或0x2,OpenOrClose指示是关闭还是打开 void SVA_DO(int ch, int iMask, int OpenOrClose) { if ((iMask & 0x1) != 0) { if (OpenOrClose == 0) { SetupOutputOpenCh(ch,0x1); } else { SetupOutputCloseCh(ch,0x1); } } if ((iMask & 0x2) != 0) { if (OpenOrClose == 0) { SetupOutputOpenCh(ch,0x2); } else { SetupOutputCloseCh(ch,0x2); } } } //伺服电机驱动卡初始化,ich为驱动卡地址0,1,2,3 void InitSV(int iCh) { int iAddr; iAddr = GetAddrFromCh(iCh); poke(iAddr,31); wait(0.005); poke(iAddr,0); wait(0.005); poke(iAddr,0); wait(0.005); poke(iAddr,0); wait(0.005); poke(iAddr,0); wait(0.005); poke(iAddr,28); wait(0.005); return; } //伺服电机驱动卡角度设定，ich为驱动卡地址0,1,2,3，index为电机编号(0-6),status为状态(0:禁用,1正转,2反转)，G为角度0-90度*/ void SetServo(int iCh,int index,int status,int G) { int iAddr; iAddr = GetAddrFromCh(iCh); poke(iAddr,31); wait(0.005); poke(iAddr,index); wait(0.005); poke(iAddr,status); wait(0.005); poke(iAddr,(2*G)/16); wait(0.005); poke(iAddr,(2*G)&0xF); wait(0.005); poke(iAddr,30); wait(0.005); return; } //大功率电机驱动设定,ich为驱动卡地址0,1,2,3.index为电机编号(0电机１,1电机２)，status为电机状态（０停，１正转，２反转），v为速度（0-100） void MoveMotor(int iCh,int index,int status,int V) { int iAddr; iAddr = GetAddrFromCh(iCh); poke(iAddr,31); //wait(0.005); poke(iAddr,index); //wait(0.005); poke(iAddr,status); //wait(0.005); poke(iAddr,V/16); //wait(0.005); poke(iAddr,V&0xF); //wait(0.005); poke(iAddr,29); //wait(0.005); return; } //大功率电机驱动设定,ich为驱动卡地址0,1,2,3.index为电机编号(0无，1电机１,2电机２,3电机１和２)，status为电机状态（０停，１正转，２反转），v为速度（0-100） void SetMotor(int iCh,int index,int status,int V) { if(index == 1) MoveMotor(iCh,0,status,V); else if(index == 2) MoveMotor(iCh,1,status,V); else if(index == 3) { MoveMotor(iCh,0,status,V); MoveMotor(iCh,1,status,V); } return; } //读取指南针,返回0-360。该函数使用前需要调用初始化串口函数 int Compass_Degree() { return SCI_Receive(0)*2; }