📄 processcmds.c
字号:
// Process laptop commands ProcessCmds.c // Rev 12/31/05
//Copyright (C) 2005 Alex Brown rbirac@cox.net
//This program is free software; See license at the end of this file for details.
/*
Reads messages from laptop and processes them to set appropriate modes and
parameters.
Handles motion command message, Drive parameters and Configuration parameters
*/
#include "Microcontroller.h" //for registers, config data & stdio
extern int USBtimeout;
extern int ucStatus;
extern int MotorType;
//Motion parameters
int seqnumUSB;
int CmdTypeUSB;
int FwdModeUSB;
int FwdAccMaxUSB;
int FwdVmaxUSB;
long FwdDistanceUSB;
int TurnModeUSB;
int TurnAccMaxUSB;
int TurnVmaxUSB;
long TurnDistanceUSB;
int seqnumCurrent;
int FwdCmdCurrent;
int TurnCmdCurrent;
int DIOcmd; //discrete I/O command
int param0,param1,param2; //RC servo parameters
int i; //loop counter
//Drive constants
extern int PwmBaseL; //motor start threshold (motor)
extern int KvL; //velocity gain (feedforward) (motor)
extern int KaccL; //acceleration gain (feedforward) (motor)
extern int KdistL; //distance error gain (proportional) (motor)
extern int PwmBaseR; //motor start threshold (motor)
extern int KvR; //velocity gain (motor)
extern int KaccR; //acceleration gain (motor)
extern int KdistR; //distance error gain (motor)
extern int RCoffsetL; //left motor RC signal offset (motor)
extern int RCoffsetR; //right motor RC signal offset (motor)
extern int PWMmax; //max duty cycle value (motor)
extern int PivotConv; //degrees to distance conversion (motor)
extern long odomLnum; //wheel calibration values (Navigation)
extern long odomLdenom;
extern long odomRnum;
extern long odomRdenom;
extern int NumSonars;
extern int AnaCfg0,AnaCfg1,AnaCfg2,AnaCfg3,AnaCfg4,AnaCfg5,AnaCfg6;
extern int AnaCfg7,AnaCfg8,AnaCfg9,AnaCfg10,AnaCfg11,AnaCfg12;
extern int RCcfgGain0,RCcfgGain1,RCcfgGain2,RCcfgGain3,RCcfgGain4,RCcfgGain5,
RCcfgGain6;
extern int RCcfgOff0,RCcfgOff1,RCcfgOff2,RCcfgOff3,RCcfgOff4,RCcfgOff5,
RCcfgOff6;
//extern int buffinW[];
extern char BuffIn[];
extern int Vmax;
extern int DistTarget;
extern int mode;
extern int MotorMode;
extern int time;
extern int hdgdeg; //from DG
extern long Xcoord,Ycoord; //from Navigation
extern int DIOcfg;
extern int DataLogOn; // 1 = logging on, 0 = off
extern int DataCase; // select data set 0 to 15
//external function prototypes
void TimerInit(void);
void MotorFwd(int,int,long int,int); //motor.c
void MotorStop(int);
void MotorTurnDeg(int,int,long);
void MotorTurnDegMod(int,int,long);
void MotorTurnStop(int);
void MotorHdgSel(int,int,long);
void MotorHdgHold(int,int,long);
int Word(char*); //converts 2 bytes in USB buffer to word(int)
long Long(char*); //converts 4 bytes in USB buffer to long
void hdgset(int);
void SetX(long);
void SetY(long);
void DIOset(int DIOcmd);
void DIOinit(void);
void RCservoGoTo(int num, int cmd, int rate);
void RCservoScan(int num, int max, int min, int rate);
void motorinit(void);
// internal prototypes
void ProcessCmdMsg(void); //process motion command message
void ProcessDriveMsg(void); //process drive system parameters
void ProcessConfigMsg1(void); //process configuration data
void ProcessCalibMsg1(void); //process calibration data
\
//--------------------------------------------------------------------------
void ProcessCmds(void)
{
if(seqnumCurrent != seqnumUSB) //if message has not been previously seen
{
seqnumCurrent = seqnumUSB;
CmdTypeUSB = Word(&BuffIn[4]);
printf("CmdType %d fwdmode %d %d %d %ld \n",CmdTypeUSB,Word(&BuffIn[8]),Word(&BuffIn[12]),Word(&BuffIn[14]),Long(&BuffIn[16]));
// printf(" %c %c %c %c \n",BuffIn[16],BuffIn[17],BuffIn[18],BuffIn[19]);
switch(CmdTypeUSB) //select type of command message handler
{
case 0:
// only process if several laptop messages have been received
// to avoid a possible motion transient when laptop is restarted
if(USBtimeout <= USBtimeoutOK)
ProcessCmdMsg(); //process motion command message
break;
case 1000:
ProcessDriveMsg(); //process drive system parameters
break;
case 1001:
ProcessConfigMsg1(); //process configuration data
break;
case 1002:
ProcessCalibMsg1(); //process calibration data
break;
}
}
}
//--------------------------------------------------------------------------
void ProcessDriveMsg(void) //process drive configuration message
{
// Initialize motor drive for RC control or H-bridge
if(Word(&BuffIn[40]) == 1) MotorType = 1; else MotorType = 0;
motorinit();
//Left motor constants
PwmBaseL = Word(&BuffIn[ 8]); //motor start threshold
KvL = Word(&BuffIn[10]); //velocity gain (feedforward)
KaccL = Word(&BuffIn[12]); //acceleration gain (feedforward)
KdistL = Word(&BuffIn[14]); //distance error gain (proportional)
RCoffsetL = Word(&BuffIn[36]); // RC signal null offset
//Right motor constants
PwmBaseR = Word(&BuffIn[16]); //motor start threshold
KvR = Word(&BuffIn[18]); //velocity gain
KaccR = Word(&BuffIn[20]); //acceleration gain
KdistR = Word(&BuffIn[22]); //distance error gain
RCoffsetR = Word(&BuffIn[38]); // RC signal null offset
//Common constants
PWMmax = Word(&BuffIn[24]); //max duty cycle value
printf("PC %d %d %d\n",KvL,KaccL,KdistL);
// Steering constants.
PivotConv = Word(&BuffIn[26]); //converts deg of rotation to wheel dist
// Wheel calibration values
odomLnum = (long) Word(&BuffIn[28]); //left conversion numerator
odomLdenom = (long) Word(&BuffIn[30]); //left conversion denominator
odomRnum = (long) Word(&BuffIn[32]); //right conversion numerator
odomRdenom = (long) Word(&BuffIn[34]); //right conversion denominator
ucStatus |= 0x8000; //set drive params flag
//printf("pc mo %d %d %d %d %d %d %d %d %d\n",PwmBaseL,KvL,KaccL,KdistL,PwmBaseR,KvR,KaccR,KdistR,PWMmax);
//printf("ucstatus after drive %x\n",ucStatus);
}
//--------------------------------------------------------------------------
void ProcessConfigMsg1(void) //process micro configuration message
{
NumSonars = Word(&BuffIn[8]);
DIOcfg = Word(&BuffIn[10]);
AnaCfg0 = Word(&BuffIn[12]);
AnaCfg1 = Word(&BuffIn[14]);
AnaCfg2 = Word(&BuffIn[16]);
AnaCfg3 = Word(&BuffIn[18]);
AnaCfg4 = Word(&BuffIn[20]);
AnaCfg5 = Word(&BuffIn[22]);
AnaCfg6 = Word(&BuffIn[24]);
AnaCfg7 = Word(&BuffIn[26]);
AnaCfg8 = Word(&BuffIn[28]); // battery voltage
AnaCfg9 = Word(&BuffIn[30]); // not connected
AnaCfg10 = Word(&BuffIn[32]); // not connected
AnaCfg11 = Word(&BuffIn[34]); // not connected
AnaCfg12 = Word(&BuffIn[36]); // not connected
RCcfgGain0 = Word(&BuffIn[38]); // RC servo calib gains
RCcfgOff0 = Word(&BuffIn[40])*61; // and offsets
RCcfgGain1 = Word(&BuffIn[42]);
RCcfgOff1 = Word(&BuffIn[44])*61;
RCcfgGain2 = Word(&BuffIn[46]);
RCcfgOff2 = Word(&BuffIn[48])*61;
RCcfgGain3 = Word(&BuffIn[50]);
RCcfgOff3 = Word(&BuffIn[52])*61;
RCcfgGain4 = Word(&BuffIn[54]);
RCcfgOff4 = Word(&BuffIn[56])*61;
RCcfgGain5 = Word(&BuffIn[58]);
RCcfgOff5 = Word(&BuffIn[60])*61;
RCcfgGain6 = Word(&BuffIn[62]);
RCcfgOff6 = Word(&BuffIn[64])*61;
TimerInit(); //set up sonars and RCservo ports
DIOinit();
ucStatus |= 0x4000; //set config1 status flag
//printf("ucstatus after config %x\n",ucStatus);
//printf("pc cfg %d %d %d %d \n",RCcfgGain0,RCcfgOff0,RCcfgGain1,RCcfgOff1);
//printf("pc cfg %d %d %d %d \n",RCcfgGain2,RCcfgOff2,RCcfgGain3,RCcfgOff3);
//printf("pc cfg %d %d %d %d \n",RCcfgGain4,RCcfgOff4,RCcfgGain5,RCcfgOff5);
//printf("pc cfg %d %d \n",RCcfgGain6,RCcfgOff6);
}
//--------------------------------------------------------------------------
void ProcessCalibMsg1(void) //process micro calibration message
{
/* RCcfgL0 = Word(&BuffIn[8]); delete this message???
RCcfgR0 = Word(&BuffIn[10]);
RCcfgL1 = Word(&BuffIn[12]);
RCcfgR1 = Word(&BuffIn[14]);
RCcfgL2 = Word(&BuffIn[16]);
RCcfgR2 = Word(&BuffIn[18]);
RCcfgL3 = Word(&BuffIn[20]);
RCcfgR3 = Word(&BuffIn[22]);
RCcfgL4 = Word(&BuffIn[24]);
RCcfgR4 = Word(&BuffIn[26]);
RCcfgL5 = Word(&BuffIn[28]);
RCcfgR5 = Word(&BuffIn[30]);
RCcfgL6 = Word(&BuffIn[32]);
RCcfgR6 = Word(&BuffIn[34]);
*/
ucStatus |= 0x2000; //set calib1 status flag
//printf("ucstatus after calib %x\n",ucStatus);
//printf("pc cfg %d %d %d %d \n",RCcfgL0,RCcfgR0,RCcfgL6,RCcfgR6);
}
//--------------------------------------------------------------------------
void ProcessCmdMsg(void) // process motion command message
{
int temp;
//Process motion commands
FwdModeUSB = Word(&BuffIn[8]);
FwdAccMaxUSB = Word(&BuffIn[12]);
FwdVmaxUSB = Word(&BuffIn[14]);
FwdDistanceUSB = Long(&BuffIn[16]);
TurnModeUSB = Word(&BuffIn[20]);
TurnAccMaxUSB = Word(&BuffIn[24]);
TurnVmaxUSB = Word(&BuffIn[26]);
TurnDistanceUSB = Long(&BuffIn[28]);
//printf("PC7 %d %d %d %ld %d %d %ld\n",seqnumUSB, FwdModeUSB, FwdVmaxUSB,
//FwdDistanceUSB, TurnModeUSB,TurnVmaxUSB,TurnDistanceUSB);
//Discrete I/O command
DIOcmd = Word(&BuffIn[44]);
//Datalog command
temp = Word(&BuffIn[88]); //bits 14 & 15 are datalog off and on select
if((temp & 0x8000)&&(DataLogOn == 0))
{ DataLogOn = 1; // turn on logging
DataCase = temp & 0x00FF; // set data set selection
ucStatus |= 0x0800; // set status flag on
}
if(temp & 0x4000)
{ DataLogOn = 0; // turn off logging
ucStatus &= ~0x0800; // set status flag off
}
//Process RC servo commands
for(i = 0; i<7; i++)
{
int temp;
param0 = Word(&BuffIn[46+6*i]); //servo rate (0 means no new command)
param1 = Word(&BuffIn[46+6*i+2]); //right limit for scan, else goto cmd
param2 = Word(&BuffIn[46+6*i+4]); //left limit for scan, 9999 for goto
if(param0 != 0) //is a new commmand
{
if(param2 == 9999) //is a servo goto command
RCservoGoTo(i,param0,param1);
else // else is a servo scan command
RCservoScan(i,param0,param1,param2);
}
}
//printf("PC3.. %d %d %d %d\n",FwdModeUSB,FwdAccMaxUSB,FwdVmaxUSB,FwdDistanceUSB);
switch (FwdModeUSB)
{
case 1: //go fwd/rev (new command from zero distance)
MotorFwd(FwdAccMaxUSB,FwdVmaxUSB,FwdDistanceUSB,1);
FwdCmdCurrent = seqnumCurrent;
break;
case 3: //change parameters of ongoing fwd/rev command
MotorFwd(FwdAccMaxUSB,FwdVmaxUSB,FwdDistanceUSB,0);
FwdCmdCurrent = seqnumCurrent;
break;
case 5: //stop
MotorStop(FwdAccMaxUSB);
FwdCmdCurrent = seqnumCurrent;
break;
} //end FwdModeUSB switch
switch (TurnModeUSB)
{
case 1: //turn delta degrees
MotorTurnDeg(TurnAccMaxUSB,TurnVmaxUSB,TurnDistanceUSB);
TurnCmdCurrent = seqnumCurrent;
break;
case 3: //change parameters of ongoing turn command
MotorTurnDegMod(TurnAccMaxUSB,TurnVmaxUSB,TurnDistanceUSB);
TurnCmdCurrent = seqnumCurrent;
break;
case 5: //stop
MotorTurnStop(TurnAccMaxUSB);
TurnCmdCurrent = seqnumCurrent;
break;
case 10: //turn to specified heading
MotorHdgSel(TurnAccMaxUSB,TurnVmaxUSB,TurnDistanceUSB);
TurnCmdCurrent = seqnumCurrent;
break;
case 11: //hold current heading (+/- optional offset)
MotorHdgHold(TurnAccMaxUSB,TurnVmaxUSB,TurnDistanceUSB);
TurnCmdCurrent = seqnumCurrent;
break;
//case 20: turn at specified radius to specified heading
//case 21: turn at specified radius to incremental heading.
} //end TurnModeUSB switch
// Set DIO output bits
DIOset(DIOcmd);
//set X,Y and heading
/* NOT WORKING YET
if(Word(&BuffIn[2]) & 0x8000) //if bit 15 of heading word set
{if(Word(&BuffIn[2]) & 0x4000) //if set absolute value is set
{hdgset(((Word(&BuffIn[2]) & 0x3FFF)<<2)>>2);
}
else //else set relative to current heading
{hdgset((hdgdeg +(Word(&BuffIn[2]) & 0x3FFF)<<2)>>2);
}
}
if(Word(&BuffIn[2]) & 0x8000) //if bit 15 of Xcoord is set
{if(Word(&BuffIn[2]) & 0x4000) //if set absolute value is set
{SetX(((Long(&BuffIn[2]) & 0x3fffffff)<<2)>>2);
}
else //else set relative to current X
{ SetX(((Long(&BuffIn[2]) & 0x3fffffff)<<2)>>2 + Xcoord);
}
}
if(Word(&BuffIn[2]) & 0x8000) //if bit 15 of Ycoord is set
{if(Word(&BuffIn[2]) & 0x4000) //if set absolute value is set
{SetY(((Long(&BuffIn[2]) & 0x3fffffff)<<2)>>2) ;
}
else //else set relative to current X
{ SetY(((Long(&BuffIn[2]) & 0x3fffffff)<<2)>>2 + Ycoord);
}
}
} //end if (seqnum...)
*/
} //end function
// OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use,
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the
Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/ ⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -