void TurnMoveLeft(void) {
MotorControl( RIGHT_MOTOR, P_CCW_SPEED_TURN );
// MotorControl( RIGHT_MOTOR, P_CCW_SPEED_NOMAL );
MotorControl( LEFT_MOTOR, P_CCW_SPEED_TURN );
// MotorControl( LEFT_MOTOR, 0 );
// MotorControl( LEFT_MOTOR, P_CCW_SPEED_NOMAL );
}
void TurnMoveRight(void) {
MotorControl( RIGHT_MOTOR, P_CW_SPEED_TURN );
// MotorControl( RIGHT_MOTOR, 1024 );
// MotorControl( RIGHT_MOTOR, P_CW_SPEED_NOMAL );
MotorControl( LEFT_MOTOR, P_CW_SPEED_TURN );
// MotorControl( LEFT_MOTOR, P_CW_SPEED_NOMAL );
}
void motorSetup()
{
//motorArray[0].attach(3);
motorArray[1].attach(5);
//motorArray[2].attach(6);
//motorArray[3].attach(9);
for(int i = 0; i<4;i++)
{
motorSpeedHover[i] = min_speed - 30 ;
motorSpeedCorrection[i] = motorSpeedHover[i];
motorSpeedPrevious[i] = 0xFFFF;
}
MotorControl();
}
int main(void){
int log = 0;
//Start Switch
// DDRA = 0x00;
// PORTA = 0x12;
//Start PORT A for switch and IR sensors
DDRA = 0xFC;
PORTA = 0xFE;
//LED Initial
DDRC = 0x7F;
PORTC = 0x7E;
DDRD = 0x70;
PORTD = 0x11;
MotorInit();
initSerial();
char * readData = NULL;
int isFinish = 0;
sensorInit();
if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
while(1){
sensorTest(0);
sensorTest(1);
sensorTest(2);
setMode();
if( checkSerialRead() > 0 ){
readData = getReadBuffer();
if( readData != NULL ){
// printf( "readData=%s\n", &readData[0] );
split( &readData[0] );
switch( serCmd[0] ){
case EVT_ACTION:
ServoControl( serCmd[1] );
// setSpeedTest( serCmd[1] );
sendAck(1);
break;
case EVT_START_MOTION:
startMotion( serCmd[1], serCmd[2] );
PORTC = ~(1 << (LED_MAX - 2));
sendAck(1);
break;
case EVT_STOP_MOTION:
stopMotion();
sendAck(1);
break;
case EVT_FORCE_MOTION:
forceMotion( serCmd[1], serCmd[2] );
break;
case EVT_GET_NOW_ANGLE:
getAngle();
break;
case EVT_SET_ANGLE:
setAngle();
case EVT_GET_ACT_ANGLE:
if( serCmd[1] >= ACT_MAX ){
sendAck(0);
}else{
sendActAngle(serCmd[1]);
}
break;
case EVT_GET_LOAD:
getLoad();
// printf( "%d\n", movingTime );
break;
case EVT_GET_VOLTAGE:
getVoltage();
break;
case EVT_TORQUE_DISABLE:
dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
break;
case EVT_WATCH_DOG:
watchDogCnt = 0;
break;
case EVT_MOTION_EDIT:
break;
case 999:
// printf( "finish\n");
sendAck(999);
isFinish = 1;
break;
default:
sendAck(0);
}
if( isFinish > 0 ){
MotorControl( 0, 0 );
break;
}
memset( readData, 0x00, SERIAL_BUFFER_SIZE );
}
}
memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE );
if (~PINA & SW_START ) {
if (log == 1) printf( "main() 0\n");
if( iStart > 0 ){
iStart = 0;
PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
if (isCaptureMode != 1) ServoControl( 0 );
}
}else{
if( iStart == 0 ){
PORTC &= ~LED_PLAY;
iStart = 1;
}
if( modeWait <= 0 ){
if (log == 1) printf( "main() 1\n");
setModeAction();
if (isMovetest == 1) {
moveTest();
} else {
move();
}
}else{
if (log == 1) printf( "main() 2\n");
modeWait -= MAIN_DELAY;
}
}
if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) {
if (log == 1) printf( "### main() sensorValue[0] == 0\n");
PORTC |= LED_PROGRAM; //edit
}else if (sensorValueOld[0] != sensorValue[0]){
if (log == 1) printf( "### main() sensorValue[0] == 1\n");
PORTC &= ~LED_PROGRAM; //edit
}
if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) {
if (log == 1) printf( "### main() sensorValue[1] == 0\n");
PORTC |= LED_MANAGE; //mon
}else if (sensorValueOld[1] != sensorValue[1]){
if (log == 1) printf( "### main() sensorValue[1] == 1\n");
PORTC &= ~LED_MANAGE; //mon
}
if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) {
if (log == 1) printf( "### main() sensorValue[2] == 0\n");
PORTC |= LED_AUX; //AUX
}else if (sensorValueOld[2] != sensorValue[2]){
if (log == 1) printf( "### main() sensorValue[2] == 1\n");
PORTC &= ~LED_AUX; //AUX
}
sensorValueOld[0] = sensorValue[0];
sensorValueOld[1] = sensorValue[1];
sensorValueOld[2] = sensorValue[2];
// walk pattern LED
// brinkLED();
_delay_ms(MAIN_DELAY);
watchDogCnt++;
caputureCount1++;
if (caputureCount1 == 25){
if (isCaptureMode == 1) getAngle();
caputureCount1 = 0;
}
}
}
void BackMove(void) {
MotorControl( RIGHT_MOTOR, P_CW_SPEED_TURN );
MotorControl( LEFT_MOTOR, P_CCW_SPEED_TURN );
}
void TurnLowMoveLeft(void) {
MotorControl( RIGHT_MOTOR, P_CCW_SPEED_TURN_2 );
MotorControl( LEFT_MOTOR, P_CCW_SPEED_TURN_2 );
}
void StraightMoveLeftSift2(void) {
MotorControl( RIGHT_MOTOR, P_CCW_SPEED_NOMAL );
MotorControl( LEFT_MOTOR, P_CW_SPEED_TURN_2 );
}
void StraightMoveRightSift(void) {
MotorControl( RIGHT_MOTOR, P_CCW_SPEED_TURN );
MotorControl( LEFT_MOTOR, P_CW_SPEED_NOMAL );
}
void StopMove(void) {
MotorControl( RIGHT_MOTOR, 1024 );
MotorControl( LEFT_MOTOR, 0 );
}
void setParamMoveAction(int right, int left) {
MotorControl( RIGHT_MOTOR, right );
MotorControl( LEFT_MOTOR, left );
}
// 核心进程
void CoreControl(void) {
INT16U i = 0, j;
INT8U tpls = 0;
// INT16U tmp;
// 串口发送的状态, 和串口字
//INT8U sciState = 0;//, sciChar;
StartSpeeder();
ClearDistanceCounter();
MotorControlInit();
crsN = 0;
nowLoop = 0;
FOREVER() {
//PORTB = ~PORTB;
// 判断小按键是否按下, 是则进入菜单
if (!PTIP_PTIP0) {
WaitEnable();
Wait(20);
if (!PTIP_PTIP0) {
StopRun();
StartMenu();
WaitSmallButtonPress();
Wait(1500);
}
}
// 采集红外值
for (i = 0;i < nIR / 2;i++) {
irSendDouble(irRevPair[i][0], irRevPair[i][1]);
for (j = 0;j < 100;j++);
ReadADCDouble(irRevPair[i][2],irRevPair[i][3], &wir[irRevPair[i][0]], &wir[irRevPair[i][1]]);
}
// 将各值归一化
for (i = 0;i < nIR;i++) {
if (whiteAvg[i] < wir[i]) {
irValue[i] = 100;
} else if (blackAvg[i] > wir[i]) {
irValue[i] = 0;
} else {
irValue[i] = (100 * (wir[i] - blackAvg[i])) / ( whiteAvg[i] - blackAvg[i]);
}
}
/** 红外值的手工修正 **/
//irValue[2] = irValue[2] * 100 / 94;
//irValue[3] = irValue[3] * 100 / 86;
//irValue[6] = irValue[6] * 100 / 94;
// 找最低
minIRv = 100;
maxIRv = 0;
ttotal = 0;
for (i = 0;i < nIR;i++) {
if ( minIRv > irValue[i] ) {
minIRn2 = minIRn;
minIRn = i;
minIRv2 = minIRv;
minIRv = irValue[i];
}
if ( maxIRv < irValue[i] ) {
maxIRv = irValue[i];
}
ttotal+= irValue[i];
}
total = ttotal;
// 求出精确位置
if ( minIRn <= (nIR - 1 - 1) && minIRn >= 1) {
position = ir_position[minIRn - 1] + irValue[minIRn - 1] * IR_SPACE_BETWEEN / (irValue[minIRn - 1] + irValue[minIRn + 1]);
} else if (minIRn == 0) {
if (irValue[1] >= 90) {
position = ir_position[0] - (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
} else {
position = ir_position[0] + (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]);
}
} else {
if (irValue[nIR - 1 - 1] >= 92/** TODO 这样合适么? **/) {
position = ir_position[nIR - 1] + (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
} else {
position = ir_position[nIR - 1] - (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]);
}
}
// 排错, 理论上前后两次之间的值相差不应超过15
if ( ((position > last_position)?(position - last_position):(last_position - position)) > 60) {
if (nowLoop == 1) {
if (GetDistance() - lastAllWhiteDist > 50)
position = last_position;
} else if (nowLoop == 2) {
if (GetDistance() - (StartLineDist[1] - StartLineDist[0]) - lastAllWhiteDist > 50)
position = last_position;
}
}
// 类模糊法判断各种路面情况
/** TODO 各各变量的权值还可以调节(通过数据), 变量数还可以增加 **/
//情况 minIRv minIRv2 maxIRv total
blStateArr[NORMAL] = (100 - minIRv) + (100 - minIRv2) + maxIRv + total / nIR;
blStateArr[LOST] = minIRv + minIRv2 + maxIRv + (100 - total / nIR);
blStateArr[CROSS] = (100 - minIRv) + (100 - minIRv2) + (100 - maxIRv) + (100 - total / nIR);
blStateArr[START] = (100 - minIRv) + (100 - minIRv2) + maxIRv + (100 - total / nIR);
// 找最有可能的情况
blState = NORMAL;
for (i = 1;i < 4;i++) {
if (blStateArr[blState] < blStateArr[i]) {
blState = i;
}
}
tBlState = blState;
// 根据判断结果作出相应
if ( blState != NORMAL) {
ProcessSpecialPoint();
if (GetDistance() - lastAllWhiteDist < 50) {
last_position = position;
} else {
position = last_position;
}
} else {
last_position = position;
}
// 得出舵机转角
#if nIR == 8
// 与以前的匹配
if (position > 125) {
position = 125 + (position - 125) * 220 / 175;
} else {
position = 125 - (125 - position) * 220 / 175;
}
#endif
/** TODO 使用一个函数发生器来为舵机提供转角 **/
servoTgtAngle = (INT8U)PosToAgl(position);
//累加舵机角度
if (PerDistSrvTotal < 0xFFFFFF && PerDistSrvN < 0xFFF0 && servoTgtAngle < 130 && servoTgtAngle > 50 ) {
PerDistSrvTotal += servoTgtAngle;
PerDistSrvN++;
}
// 简单转速控制
tgtSpeed = (INT16S)(minSpeed + (maxSpeed - minSpeed) * (speed_arr[position] ) / 40);
if (tgtSpeed < 50 || tgtSpeed > XSpeed) {
tgtSpeed = lastTgtSpeed;
} else {
lastTgtSpeed = tgtSpeed;
}
// 转动舵机
ServoControl();
// 转动马达, 有限制距离的
if (DistLimit != 0 && DistLimit * 25 < GetDistance() ) {
StopRun();
} else {
MotorControl();
}
tdist = GetDistance();
/*if(SCI0SR1 & 0x80) {
switch(sciState) {
case 0:
sciChar = 0xFE;
break;
case 1:
sciChar = (INT8U)position;
break;
case 2:
sciChar = (INT8U)servoTgtAngle;
break;
case 3:
sciChar = ((INT8U)((3927 * 4) / GetSpeed()) * _RTI_P )& 0xFF;
break;
case 4:
sciChar = (INT8U)(GetDistance());
default:
break;
}
sciState = (INT16U)(sciState + 1) % 5;
SCI0DRL = sciChar;
} */
///////////////////////////////////////////////////////////////////////
}
}
int main(void) {
SetAdc(PA0);
SetAdc(PA1);
SetAdc(PA2);
SetAdc(PA3);
SetAdc(PA4);
mySonarL.SetFloorLevel(0);
myLCD.BacklightOn(0);
myLCD.SetBacklight (255);
Pause(1);
int LeftSpeed = ConstantSpeed;
int RightSpeed = ConstantSpeed;
ControlStyle myStyle=none;
SteeringCommand mySteeringCommand=neutral;
int currentStep=0;
int commandExpiration=0;
SteeringCommand commandLib[LIB_SIZE+1];
commandLib[0]=GoStraight;
commandLib[1]=GoLeft;
commandLib[2]=GoLeft;
commandLib[3]=GoLeft;
commandLib[4]=GoRight;
commandLib[5]=GoLeft;
commandLib[6]=GoRight;
commandLib[7]=GoLeft;
commandLib[8]=GoRight;
/////////////////////
//
/////////////////////
commandLib[9]=GoLeft;
commandLib[10]=GoLeft;
commandLib[11]=GoLeft;
commandLib[12]=GoStraight;
//////////////////////
//valley
//////////////////////
commandLib[13]=GoStraight;
commandLib[14]=GoLeft;
commandLib[15]=FollowCompass;
commandLib[16]=Goal;
int whichOrderToTake=1;
//black_count=0;
compassStage myStage=first_charge;
/////////////////////////
unsigned short T = 0, \
PrevT = 0, \
Multiplier = 0;
float Time = 0, \
PrevTime = 0;
SetTm4Counter(0, 0, 7199, 10000);
//////////////////
myStyle = blindMode;
float commandExpTime=0;
////////////////////
//initial charge
///////////////////
if (whichOrderToTake<=1)
{
myLCD.Display("initial charge");
myMotorR.Forward(ConstantSpeed);
myMotorL.Forward(ConstantSpeed);
Pause(10000);
}
/////////////////////////////
while(1)
{
currentStep++;
mySonarL.Ranging();
myComp.GetAngle(Angle);
Angle = Angle % 360;
for(i = 0 ; i < 5 ; i++)
{
Value[i] = GetAdc(i);
}
if( (Value[0] < BLACK) && (Value[4] < BLACK) )
black_count++;
StatusL = mySonarL.GetDistance(1, DistanceL);
////////////////////////////////////////////////
GetTm4CounterValue(T);
if(T < PrevT)
Multiplier = Multiplier + 1;
PrevT = T;
Time = Multiplier + float(T) / 10000;
///////////////////////////////////////////////
if (myStyle!=followCompassStyle)
{
if( (Value[2]<BLACK) && ( (Value[1]<BLACK) || (Value[3]<BLACK) ) )
myStyle=track_tracing;
else if((StatusL==1)&&(DistanceL<DistanceToWallStandard))
myStyle=valleyNav;
else
myStyle=blindMode;
}
int pauseTime=1;
switch(myStyle)
{
case track_tracing :
trackTracing(Value[0],Value[4],LeftSpeed,RightSpeed,commandLib,whichOrderToTake,mySteeringCommand,currentStep,commandExpiration,pauseTime,myStyle,Time,commandExpTime);
break;
case valleyNav:
ValleyNavigation(StatusL,DistanceL,LeftSpeed,RightSpeed);
pauseTime=1;
break;
case followCompassStyle:
compassNavigation(Angle,LeftSpeed,RightSpeed,Value[0],Value[1],Value[2],Value[3],Value[4],myStage,myStyle);
break;
default:
BlindMode(LeftSpeed,RightSpeed);
break;
}
MotorControl(LeftSpeed,RightSpeed);
Pause(pauseTime);
}
}
#include <string>
#include "PWM.h"
#include "Motor.h"
#include <ctime>
#include <cstdlib>
const float MIN_SPEED = 0;
const float MAX_SPEED = 100;
const int MIN_MOTOR_PULSE_TIME = 1000;
const int MAX_MOTOR_PULSE_TIME = 2000;
const std::string PIN_MOTOR_YAW("P8_13");
const std::string PIN_MOTOR_RIGHT("P8_19");
const std::string PIN_MOTOR_LEFT("P9_14");
MotorControl motorControls[] = { MotorControl(PIN_MOTOR_LEFT, MIN_SPEED, MIN_SPEED, MAX_SPEED), MotorControl(PIN_MOTOR_RIGHT, MIN_SPEED, MIN_SPEED, MAX_SPEED), MotorControl(PIN_MOTOR_YAW, MIN_SPEED, MIN_SPEED, MAX_SPEED), };
#include <signal.h>
// Stop all the motors when we interrupt the program so they don't keep going
void sig_handler(int signum)
{
for (unsigned int iMotor = 0; iMotor < 3; iMotor++)
{
MotorControl & motor = motorControls[iMotor];
motor.SetOutputValue(0);
motor.UpdatePWMSignal();
}
exit(signum);
