예제 #1
0
void Modbus_Handler(void)
{
	uint16_t command = 0;
	if(*(usRegHoldingBuf + MODBUS_COMMAND) != 0)
	{
		command = *(usRegHoldingBuf + MODBUS_COMMAND);
		*(usRegHoldingBuf + MODBUS_COMMAND) = 0;
		
		switch(command) 
		{
			case 1:
				SaveEEPROM();
				break;//save all
			case 2:
				LoadModbus();
				break;//load
			case 3:
				LoadModbusDefault();
				break;//default
			case 4:
				SaveModbus4();
				break;//save 1
			case 5:
				SaveModbus5();
				break;//save 2
			case 6:
				SaveModbus6();
				break;//save 3
			case 7:
				EraseEEPROM();
				break;
			case 8:
				CleanFlash();
				break;
			case 9:
	// 			g_uartMode = Command;
				break;
			case 10:
	//			g_modbusReg4[modbusDefaultSet] = 0;//reboot
				break;
			
			default: 
				break;
		}
	
	}
	
}
예제 #2
0
void main()
{			
	InitHardware();
	Encoder(true);
	
	ResetServo();
	delay_ms(200);

	bEnable = !STEP_ENABLE;
		
	// run these asynch tasks
	for (;;){
		if (!STEP_ENABLE){
			HandleRXData();
			if (bDirty && !bEnable){
				bDirty = false;
				SaveEEPROM();
			}
		}
	}

}
예제 #3
0
void main()
{			
	InitHardware();
	Encoder(true);
	
	ResetServo();
	delay_ms(200);

	bEnable = !STEP_ENABLE;
		
	// run these asynch tasks
	for (;;){
		if (bServo){
			SYNC = true;
			UpdatePosition();
			UpdateTrajectory();
			CalculateExtCount();
			UpdatePID();
			UpdatePWM();
			HandleErrorTask();
			bServo = false;
		} else {		
			//HandleRXData();
			
			if (SYNC){
				if (bDirty && !bEnable){
					bDirty = false;
					SaveEEPROM();
				}
				SYNC = false;	
			}


		}
	}
}
예제 #4
0
void UpdateTrajectory()
{			
	STATUS.index = INDEX_PIN;

	if (STATUS.index && STATUS.halt_index || STATUS.error){
		velact = 0;
		return;
	}

	switch (mode){
	case e_velo_mode:
		if (!(!STATUS.halt_index && STATUS.saturated)){			 

			if ((long)Fix2LongInt(velact.ul) < vars.velcom){		// is the velocity lower than target 
				velact.sl += vars.accel;
				
				if ((long)Fix2LongInt(velact.ul) > vars.velcom)
					velact = LongInt2Fix(vars.velcom,0);
					
			} else if ((long)Fix2LongInt(velact.ul) > vars.velcom){
				velact.sl -= vars.accel;
				
				if ((long)Fix2LongInt(velact.ul) < vars.velcom)
					velact = LongInt2Fix(vars.velcom,0);
			}	
			
			cmd_pos.sl += Fix2LongInt(velact.ul);
		}
		break;

	case e_pos_mode:
		if (!(!STATUS.halt_index && STATUS.saturated)){			 

			if (!FLAGS.phase){							// process the first half of the profile 
				
				if ((long)Fix2LongInt(velact.ul) < vars.vlim)		// we need to accelerate to start move
					velact.sl += vars.accel;		// keep adding accel value to actual velocity
				else
					flatcount++;				// keep track of visits through this function
														// after accel has peaked
				phase1dist.sl -= Fix2LongInt(velact.ul); 

				if (FLAGS.neg_move)
					cmd_pos.sl -= Fix2LongInt(velact.ul);	// keep moving position
				else
					cmd_pos.sl += Fix2LongInt(velact.ul);	// keep moving position forward
				
				if (phase1dist.sl < 0)
					FLAGS.phase = true; 	// if phase distance is neg, then we are finished with first half of profile
	
			} else {
												// this is the second half of the profile
				if (flatcount > 0){
					flatcount--;
				
				} else if ((long)Fix2LongInt(velact.ul)>0 ){
					velact.sl -= vars.accel;
				
				} else {
					cmd_pos = LongInt2Fix(fposition,0);
					STATUS.move_in_prog = false;
					mode = e_hold_mode;
				}

				if (FLAGS.neg_move)							// keep moving position
					cmd_pos.sl -= Fix2LongInt(velact.ul);
				else
					cmd_pos.sl += Fix2LongInt(velact.ul);
			}
		}
		break;
	
	case e_save_eeprom_mode:
		mode = e_hold_mode;
		SaveEEPROM();
		break;
		
	case e_pos_go_mode:
		StartGoMove();
		break;
		
	case e_servo_reset_mode:
		ResetServo();
		break;
	
	case e_servo_lock_mode:
		LockServo();
		break;
	
	case e_servo_unlock_mode:
		STATUS.pid_lock 	= false;
		mode 				= e_hold_mode;
		break;
	}
}