void DoAuto(void){
int tmp = 0;
int b = 0;
int i = 0;
tmp = RefPos();
if(!tmp){
MotorStop();
return;
}
MotorStart();
for(;i<20;i++){
b = borr_auto[i];
//if
tmp = Nstep(b);
if(!tmp){
MotorStop();
return;
}
tmp = DrillHole();
if(!tmp){
MotorStop();
return;
}
}
MotorStop();
return;
}
void Move_Motor( void *pvParameters ) {
MotorsInit();
tMotorMove* motorActions = (tMotorMove*) pvParameters;
unsigned char actionIndex = 0;
while( motorActions[actionIndex].duration != 0 ) {
MotorDir( LEFT_SIDE, motorActions[actionIndex].leftDirection );
MotorDir( RIGHT_SIDE, motorActions[actionIndex].rightDirection );
MotorSpeed( LEFT_SIDE, motorActions[actionIndex].leftSpeed);
MotorSpeed( RIGHT_SIDE, motorActions[actionIndex].rightSpeed);
MotorRun(LEFT_SIDE);
MotorRun(RIGHT_SIDE);
vTaskDelay( motorActions[actionIndex].duration );
actionIndex++;
}
MotorStop(LEFT_SIDE);
MotorStop(RIGHT_SIDE);
while (1) {
vTaskDelay( 500 );
}
}
int motor_STOP(){
char str[28];
evalbot->state=WAIT;
MotorStop(LEFT_SIDE);
MotorStop(RIGHT_SIDE);
sprintf(str,"!POS:x:%4i y:%4i a:%4i",
evalbot->x,
evalbot->y,
evalbot->angle);
mess_setString(str,28);
return 1;
}
/**
* @brief This function handles Memory Manage exception.
* @param None
* @retval None
*/
void MemManage_Handler(void)
{
/* Go to infinite loop when Memory Manage exception occurs */
while (1)
{
//出现错误时停转所有电机，防止意外伤人
MotorStop();
}
}
/**
* @brief This function handles Hard Fault exception.
* @param None
* @retval None
*/
void HardFault_Handler(void)
{
/* Go to infinite loop when Hard Fault exception occurs */
while (1)
{
//出现错误时停转所有电机，防止意外伤人
MotorStop();
}
}
void DoAuto(void){
int i = 0;
if(!RefPos()){
MotorStop();
return;
}
MotorStart();
while(pattern[i] != 0xFF){
if(!Nstep(pattern[i])){
MotorStop();
return;
}
if(!DrillHole()){
MotorStop();
return;
}
i++;
}
MotorStop();
return;
}
static void sysMotorEventHandle(uint8_t id, MotorEvent_t event)
{
bool doNextStep = false;
if(MOTOR_EVENT_STEP_OVER == event)
{
doNextStep = true;
if(g_curStepInfo->flag == 1)
{
MProtoCtrlResult(MPROTO_RESULT_SENOR_ERROR); //´«¸ÐÆ÷δ´¥·¢
}
}
else if(MOTOR_EVENT_SENSOR_TRIGGERED == event)
{
if(g_curStepInfo != NULL
&& g_curStepInfo->id == id
&& g_curStepInfo->flag == 1)
{
doNextStep = true;
}
}
if(doNextStep)
{
MotorStop(id);
if(g_stepCount == 0)
{
g_sysStatus = SYS_STATUS_IDLE;
MProtoCtrlResult(MPROTO_RESULT_SUCCESS);
g_curStepInfo = NULL;
}
else
{
g_curStepInfo++;
g_stepCount--;
MotorStart(g_curStepInfo->id, g_curStepInfo->dir, g_curStepInfo->cycle);
}
}
}
void DoAuto(){
int pattern[21] = {0,1,1,1,1,1,1,1,2,1,5,2,2,2,2,4,4,3,8,2,255};
int antalSteg;
int subrutinRes;
if(RefPos()){
int i=0;
MotorStart();
while(1){
antalSteg = pattern[i];
i++;
if(antalSteg == 255)
break;
subrutinRes = Nstep(antalSteg);
if(subrutinRes == 0)
break;
subrutinRes = DrillHole();
if(subrutinRes == 0)
break;
}
}
MotorStop();
}
int _tmain(int argc, _TCHAR* argv[])
{
/*------------------------Constants---------------------------------------------------------*/
CPipe motor("MotorData",1024);
CPipe motionComplete("Stopped",1024);
char* address1 = "0"; //corresponds to motor driver 1 on ST400
char* address2 = "3"; //motor driver 3
int slew_rate = 400; //This is the maximum step rate (range is 16 to 8500 steps/sec)
int first_rate = 100; //This is the initial step rate (range is 16 to 8500 steps/sec)
int accel = 1; //This is the acceleration rate (range is 0 to 255 steps/sec^2)
int current = 300; //Motor winding current in mAs
int port = 3; //Port number varies depending on the computer used.
//Check Device Manager for proper port assignment.
int RightWheelDone = 0; //Bit 7 set to 1 when motor movement is completed
int LeftWheelDone = 0;
int jogSpeed = 500; // Jog Speed (testing purposes only)
/*------------------------------------------------------------------------------------------*/
//Attempt to open the bluetooth port to begin sending commands.
while(PortOpen(port, 9600)!=1)
{
char* error = ::RMVGetErrorMessage();
printf("Port not opened. Error=%s\n",error);
Sleep(1000);
}
printf("Port successfully opened\n");
//Initialize the stepper motors
MotorStationsInit();
//Configure the motors to half-step mode
if(MotorConfigureMode(address1,1)!=1 || MotorConfigureMode(address2,1)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Configure error=%s\n",error);
}
//Initialize the slew rate, first rate and acceleration
if(MotorSpeed( address1, slew_rate, first_rate, accel )!=1 ||
MotorSpeed( address2, slew_rate, first_rate, accel )!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Motor Speed error=%s\n",error);
}
if ( MotorSetCurrent(address1, current )!=1 || MotorSetCurrent(address2,current)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Set Current error = %s\n",error);
} else {
printf("\nMotor Current set to: %d",current);
}
//Main motor control loop. Read step instructions sent by Thursday and perform them.
while(1){
//Await instructions from Trajectory Planning program (Thursday).
motor.Read(&ST400,sizeof(ST400));
cout << "Steps (right, left) = "<< ST400.rightWheel << "\t"<<ST400.leftWheel << endl;
//Set the number of steps the motors must travel.
if(MotorNumberStepRel( address1 , -ST400.leftWheel)!=1 || MotorNumberStepRel( address2 , -ST400.rightWheel)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Step setting error=%s\n",error);
}
//Put motors in jog mode (testing purposes only) Comment out MotorNumberStepRel if using.
/*MotorJog ( address1, 1 );
MotorJog ( address2, 1 );
MotorChangeJogSpeed ( address1, jogSpeed);
MotorChangeJogSpeed ( address2, jogSpeed));*/
//Tell the motors to start executing the received command
if(MotorTriggerON(address1)!=1 || MotorTriggerON(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Trigger on error=%s\n",error);
}
//Loop until current command is completed (bit 7 is set to 1) or an obstacle is detected
while(((RightWheelDone & 0x80) != 128) || ((LeftWheelDone & 0x80) != 128)){
//Read motor status register to determine if current motion has ended
if(MotorGetStatusRegister(address1,&LeftWheelDone)!= 1 ||
MotorGetStatusRegister(address2,&RightWheelDone)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Get Status error=%s\n",error);
}
//Check if Thursday has sent data and read it
if(motor.TestForData()>=sizeof(ST400))
{
motor.Read(&ST400,sizeof(ST400));
}
//Check if Thursday has indicated motor motion should stop (i.e. if an obstacle has been
//detected by the IR sensors) and end the current motor motion if it has.
if(ST400.stop != 0)
{
//Stop the current motion
if(MotorStop(address1)!=1 || MotorStop(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("Stop error=%s\n",error);
}
ST400.stop = 0;
//Exit nested while loop
break;
}
}
//Send a 1 to Thursday to indicate current instructions are completed.
int done=1;
motionComplete.Write(&done,sizeof(done));
//Reset done variables
LeftWheelDone = 0;
RightWheelDone = 0;
MotorTriggerOFF(address1);
MotorTriggerOFF(address2);
/*
if(MotorPowerOFF(address1)!=1 || MotorPowerOFF(address2)!=1)
{
//An error has occured. Display the RMV error message to determine the error.
char* error = ::RMVGetErrorMessage();
printf("error=%s\n",error);
}*/
}
}
int
main(void)
{
unsigned long ulPHYMR0;
tBoolean bButtonWasPressed = false;
tMotorState sMotorState=STATE_STOPPED;
tWaveHeader sWaveHeader;
unsigned long ulWaveIndex = 1;
int losL,losH;
ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_ETH);
ulPHYMR0 = ROM_EthernetPHYRead(ETH_BASE, PHY_MR0);
ROM_EthernetPHYWrite(ETH_BASE, PHY_MR0, ulPHYMR0 | PHY_MR0_PWRDN);
// Display96x16x1Init(true);
// Display96x16x1StringDraw("MOTOR", 29, 0);
// Display96x16x1StringDraw("DEMO", 31, 1);
LEDsInit();
LED_On(LED_1);
PushButtonsInit();
BumpSensorsInit();
MotorsInit();
ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / 100);
ROM_SysTickEnable();
ROM_SysTickIntEnable();
SoundInit();
// WaveOpen((unsigned long *) sWaveClips[ulWaveIndex].pucWav, &sWaveHeader);
//mozliwe ze trzeba przed kazdym odtworzeniem ;/
// while(WaveOpen((unsigned long *)
// sWaveClips[ulWaveIndex].pucWav,
// &sWaveHeader) != WAVE_OK);//do zablokowania w razie bledu
for(;;)
{
tBoolean bButtonIsPressed;
tBoolean bBumperIsPressed[2];
bButtonIsPressed = !PushButtonGetDebounced((tButton)1);
bBumperIsPressed[0] = !BumpSensorGetDebounced((tBumper)0);
bBumperIsPressed[1] = !BumpSensorGetDebounced((tBumper)1);
switch(sMotorState)
{
case STATE_STOPPED:
{
if(bButtonIsPressed && !bButtonWasPressed)
{
sterowanie(0,50,50);
sMotorState = STATE_RUNNING;
}
break;
}
case STATE_RUNNING:
{
if(bButtonIsPressed && !bButtonWasPressed)
{
MotorStop((tSide)0);
MotorStop((tSide)1);
sMotorState = STATE_STOPPED;
}
else if(bBumperIsPressed[0])
{
MotorStop((tSide)0);
MotorStop((tSide)1);
// WavePlayStart(&sWaveHeader); mozliwe ze tu
losL =10+ g_ulTickCount % 20;//i dać los zamiast 15,mamy losowe skrecanie(10-30)
losH =40+ g_ulTickCount % 30;//i dać los zamiast 60,mamy losowe skrecanie(40-70)
sterowanie(1,losL,losH);
sMotorState = STATE_TYL;
}
else if(bBumperIsPressed[1]){
MotorStop((tSide)0);
MotorStop((tSide)1);
losL =10+ g_ulTickCount % 20;//i dać los zamiast 15,mamy losowe skrecanie(10-30)
losH =40+ g_ulTickCount % 30;//i dać los zamiast 60,mamy losowe skrecanie(40-70)
sterowanie(1,losH,losL);
// WavePlayStart(&sWaveHeader); mozliwe ze tu
sMotorState = STATE_TYL;
}
break;
}
case STATE_TYL:
{//cofanie tez moze byc losowe np losH+losL(50-100)+160=(210-260)
while(cofanie<250);
MotorStop((tSide)0);
MotorStop((tSide)1);
sterowanie(0,50,50);
sMotorState = STATE_RUNNING;
break;
}
default:
{
MotorStop((tSide)1);
MotorStop((tSide)0);
sMotorState = STATE_STOPPED;
break;
}
}
bButtonWasPressed = bButtonIsPressed;
}
}
