Example #1
0
void motor(){
	if(motorStateChange!=motorState){
		motorStateChange = motorState;
		{
			int8_t difference = currentPosition - lastPosition;
			if(difference > 0 && motorState == 4)
			{
				currentPosition--;
			}
			else if (difference < 0 && motorState == 3)
			{
				currentPosition++;
			}
		}
	}
	
	if(TCNT1>SECOND/speedModifier){
		TCNT1 = 0;
		switch(motorState){
			case 0:
			stopMotor();
			return;
			
			case 1:
			// motor idle...
			if(!motorInitialized){
				initMotor();
				motorInitialized = 1;
			}
			return;
			
			case 3:
			/* double forward motion */
			if(steps>0){
				steps--;
				lastPosition = currentPosition-1;
				currentPosition++;
				moveMotor();
				} else {
				motorState = 1;
			}
			return;
			
			case 4:
			/* double backward motion */
			if(steps>0){
				steps--;
				lastPosition = currentPosition+1;
				currentPosition--;
				moveMotor();
				} else {
				motorState = 1;
			}
			return;
		}
	}
}
Example #2
0
void  Motor::setMotorSpeed(int Dir1, int Dir2, int Speed) {
  if (Speed > 100)
    Speed = 100;
  if (Speed < -100)
    Speed = -100;
  speed = Speed;
  moveMotor(Dir1, Dir2, Speed * mean_speed / 100);
}
Example #3
0
void  Motor::setMotorSpeed(int Speed) {
  // +ve for CW and -ve for CCW. Speed in percentage of meanspeed
  if (Speed > 100)
    Speed = 100;
  if (Speed < -100)
    Speed = -100;
  speed = Speed;
  moveMotor(Speed * mean_speed / 100);
}
Example #4
0
/*main function to run the filter wheel from the command line */
int main(int argc, char *argv[])
{
    memset(out,0,64);
    memset(in,0,64);

    int c;

    setup(); //start the communications

    while ((c = getopt (argc, argv, "pmftoiczh")) != -1)
        switch (c)
	    {
	    if(argc==1){
	        case 'p':
		    currentPos();
		    break;
	        case 't':
		    printf("test\n");
	  	    break;
		case 'o':
		    motorOff();
		    break;
		case 'z':
		    zero();
		    break;
		case '?':
         	    if (isprint (optopt))
           	        fprintf (stderr, "Unknown option `-%c'.\n", optopt);
         	    else
           		fprintf (stderr,
                        "Unknown option character `\\x%x'.\n",
                        optopt);
			return 1;
       			default:
         		abort ();
	    }
	    if(argc==2){
		case 'm':
                    printf("%s\n", argv[2]);
                    moveMotor(argv[2]);
                    break;
		case 'f':
                    printf("%s\n", argv[2]);
                    moveToFilter(atoi(argv[2]), atoi(argv[3]));
                    break;
		}
       	     }
	return 1;
}
Example #5
0
/*
moves to filter posional number [0-5]
returns the desired encoder position
*/
int moveToFilter(int pos, int delta){
	int posArr[6]={};

	//populate array with filter encoder positions
	int x;
	for (x=0; x<6; x++) { 
		posArr[x] = delta * x;
	}

	if (pos > sizeof(posArr)/sizeof(int)){
		return -1;
	}

	char mv[12];
	
	sprintf(mv, "%d", posArr[pos]);
	moveMotor(mv);
	return posArr[pos];
}
Example #6
0
int main (int argc, char **argv)
{
  if ( argc != 2 ) /* argc should be 2 for correct execution */
    {
      /* We print argv[0] assuming it is the program name */
      printf( "usage: %s <degrees to move>\n", argv[0] );
      return 1;
    }

  /* take from: https://www.securecoding.cert.org/confluence/display/seccode/INT06-C.+Use+strtol()+or+a+related+function+to+convert+a+string+token+to+an+integer */
  const char* const c_str = argv[1];
  char *end;
  int degrees;
 
  errno = 0;
 
  const long sl = strtol(c_str, &end, 10);

  if (end == c_str) {
    fprintf(stderr, "%s: not a decimal number\n", c_str);
    return 1;
  }
  else if ('\0' != *end) {
    fprintf(stderr, "%s: extra characters at end of input: %s\n", c_str, end);
    return 1;
  }
  else if ((LONG_MIN == sl || LONG_MAX == sl) && ERANGE == errno) {
    fprintf(stderr, "%s out of range of type long\n", c_str);
    return 1;
  }
  else if (sl > INT_MAX) {
    fprintf(stderr, "%ld greater than INT_MAX\n", sl);
    return 1;
  }
  else if (sl < INT_MIN) {
    fprintf(stderr, "%ld less than INT_MIN\n", sl);
    return 1;
  }
  else {
    degrees = (int)sl;
  }

  buttonPressed = 0;
  wiringPiSetup () ;

  /* Setup the micro switch pin */
  pinMode (SWITCH_PIN, INPUT);
  pullUpDnControl (SWITCH_PIN, PUD_DOWN);
  wiringPiISR (SWITCH_PIN, INT_EDGE_RISING, &buttonPressedInt) ;

  /* setup the motor output pins */
  int pin;
  for (pin = 0 ; pin < 4 ; ++pin){
    pinMode (pin, OUTPUT);
    digitalWrite (pin, 0);
  }

  while (!buttonPressed) moveMotor(1);

  moveMotor(degrees);
  return 0;
}
Example #7
0
/**
 * set left motor(s) to drive at a given speed
 * int speed from -100 to 100
 */
void leftMotor(int speed)
{
  moveMotor(motorLeft, speed);
  // TODO: uncomment the next line for quad motor configuration
  // moveMotor(motorLeft2, speed);
}
Example #8
0
void Motor_Claire::sendPWMCommand(int command)
{
  _speed = min(abs(command), 255);
  _direction = (command >= 0) ? FORWARD : BACKWARD;
  moveMotor(_speed, _direction);
}
Example #9
0
File: main.c Project: wazuba/AFC
void stateMachine(void)
{
	
	/******************************************************************************
	* INIT State
	* Executes at boot up or when the processor is reset.
	*
	******************************************************************************/
	if (state == STATE_INIT)
	{
		initPeripherals();
		initInterrupts();
		
		unsigned int checkRead;
	
		checkRead = M24LC64FReadWord(CHECK_ADDRESS, M24LC64F_ADDRESS_0);
		__delay32(EEPROM_DELAY*10);
		
		if (checkRead)
		{
			homePos = 0;	
			M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
			__delay32(EEPROM_DELAY*10);
			M24LC64FWriteWord(CHECK_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
			__delay32(EEPROM_DELAY*10);
		}
		
		homePos = M24LC64FReadWord(HOME_ADDRESS, M24LC64F_ADDRESS_0);			/* Retrieve the stored delay value */
		//homePos = 0;
		__delay32(EEPROM_DELAY*10);
		motorPos = M24LC64FReadWord(MOTOR_POS_ADDRESS, M24LC64F_ADDRESS_0);			/* Retrieve the stored Motor Position value */
		//motorPos = 0;
		__delay32(EEPROM_DELAY*10);
		initPWM();
		initADC();
		initTMR();
		indexMotor(homePos, MAX_STEPS); //MAX_STEPS is 250, moveMotor() steps 4 motor steps at a time.
	}
	/* End of INIT State*/
	
	/******************************************************************************
	* WARM_UP State
	* As long as the warm up complete signal is not given the AFC will remain in
	* this state.
	******************************************************************************/
	
	/*if (state == STATE_WARM_UP)
	{
		triggerINT = disableINT0();
		//updateAnalogOut(homePos);
		bufferFull = 0;
		
	}*/

	/******************************************************************************
	* MAN State
	* Executes if the user selected MAN using the proper hardware control signals
	* This state enables the user to manually change configuration parameters and
	* the position of the motor.
	******************************************************************************/
	
	if (state == STATE_MAN)
	{
	
		if (triggerINT || IFS0bits.INT0IF) 			/* Disabling the external interrupt INT0, only needed in AFC state */
		{
			triggerINT = disableINT0();
		}
		
		if (!MAN_DELAY_CTRL == 1)					/* Check if we are setting Motor Position (= 1) or Delay (= 0) */
		{
			motorOrDelay = 0;
			homePos = motorPos;
			updateAnalogOut(homePos);
			M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
		}
		else
		{
			motorOrDelay = 1;
			updateAnalogOut(motorPos);
		}
	
		if (!MAN_DELAY_UP == 1)								
		{
			
			__delay32(EEPROM_DELAY*10); 			/* Debouncing (50ms Delay) can be made faster for practical application */
			if (motorOrDelay == 1)
			{
				unsigned int countTemp = 0;
				count =0;
				//do{	
				while(!MAN_DELAY_UP == 1)
				{
				//__delay32(EEPROM_DELAY*4);
				setDir = FORWARD;
				motorPos++;
				moveMotor(setDir, TIMER_PERIOD2);
				while(count==countTemp);
				countTemp++;
				updateAnalogOut(motorPos);
				}
				 
				//M24LC64FWriteWord(MOTOR_POS_ADDRESS, motorPos, M24LC64F_ADDRESS_0);
				
				//}while (count <500);
				setDir = STOP;
				moveMotor(setDir, TIMER_PERIOD2);
			}
			else
			{
				
				if (homePos >= MAX_STEPS)
					homePos = MAX_STEPS;
				else
					homePos ++; 						
			
				updateAnalogOut(homePos);
				M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
				
			}
		}
	
		if (!MAN_DELAY_DOWN == 1)
		{
			__delay32(EEPROM_DELAY*10);				/* Debouncing (50ms Delay) can be made faster for practical application */
			if (motorOrDelay == 1)
			{
				unsigned int countTemp = 0;
				count =0;
				//do{	
				while(!MAN_DELAY_DOWN == 1)
				{
				//__delay32(EEPROM_DELAY*4);
				setDir = REVERSE;
				motorPos--;
				moveMotor(setDir, TIMER_PERIOD2);
				while(count==countTemp);
				countTemp++;
				updateAnalogOut(motorPos);
				}
				 
				//M24LC64FWriteWord(MOTOR_POS_ADDRESS, motorPos, M24LC64F_ADDRESS_0);
				
				//}while (count <500);
				setDir = STOP;
				moveMotor(setDir, TIMER_PERIOD2);				
			}
			else
			{
				
			
				if (homePos <= MIN_DELAY)
					homePos = 0;
				else
					homePos --;							
				
				updateAnalogOut(homePos);
				M24LC64FWriteWord(HOME_ADDRESS, homePos ,M24LC64F_ADDRESS_0);
			}
		}
	
	}
	/* End of MAN State*/
	
		
	/******************************************************************************
	* AFC State
	* Executes if the user selected AFC using the proper hardware control signals
	* This state locks all manual control and the AFC controls the motor position
	******************************************************************************/
	
	if (state == STATE_AFC)
	{
		if (!MAN_DELAY_UP == 1)
		{
			indexMotor(motorPos, MAX_STEPS);
		}
		if (!MAN_DELAY_DOWN == 1)
		{
			indexMotor(homePos, MAX_STEPS);
		}
		
		heatPerPulse = 0;
		
		if (sampleTrigger)
		{
			sampleTrigger = 0;
			
			prevReflectedPower = 0;
			
			/* The ADC is triggered by a special comparison event of the PWM module */
			ADCPC2bits.SWTRG5 = 1; /*Trigger ADC to convert AN11 (Heat Per Pulse input from PLC) */
			while(!ADSTATbits.P5RDY);
		
			heatPerPulse = ADCBUF11;
			
			ADCPC0bits.SWTRG0 = 1; /*Trigger ADC to convert AN0 (Reflected port) */
			while(!ADSTATbits.P0RDY);
			
			prevReflectedPower = reflectedPower;
			
			reflectedPower = ADCBUF0;
			
			// delta = ADCBUF1;
			
			ADSTATbits.P0RDY = 0;           /* Clear the ADSTAT bits */
			//ADSTATbits.P1RDY = 0;
			ADSTATbits.P5RDY = 0;
			 
			/*__asm__ volatile ("clr B");
			__asm__ volatile ("lac %0,B":"+r"(heatPerPulse));
			__asm__ volatile ("sftac B,#16");
			__asm__ volatile ("add A");*/
			
			heatAccumulator += heatPerPulse;
						
			/*No buffer*/
			error = (int)(reflectedPower - prevReflectedPower);
			
			/* Filtering using a FIFO buffer*/
			
			/*errorArray[strPTR] = (int)(reflectedPower - prevReflectedPower);
			strPTR++;
			if (strPTR >= endPTR)
				{
					bufferFull = 1;
					strPTR = 0;
				}
			if (bufferFull == 1)
				{
					int i;
					for (i = 0; i < endPTR; i++)
					{
						error += errorArray[i];
					}
					error = error >> 5; //BUFFER_SIZE = 32 -> 2^5 = 32
				}*/
		/*	else
				{
					int j;
					for (j = 0; j < strPTR; j++)
					{
						error += errorArray[j];
					}
					error = error/strPTR;
				}*/
			triggerINT = enableINT0();
			//calcError();						/* Calculate Error based on A/D results for reflected power*/
			//moveMotor(setDir, motorStep);
			//while (T2CONbits.TON);
		}

			
			if (thermalCounter == 50000) //Using the PWM special event interrupt to increment every TMR1 period match (~20us) 5000*20uS ~= 100mS
			{
				
				/*Reduce the accumulator by the cool rate coef.*/
				heatAccumulator -= (heatAccumulator>>COOL_SHIFTS)*COOL_RATE;

				
			}		
				/* Outer loop, calculates Thermal Drift effects and moves motor proactivley (Feedforward)*/
				target = homePos + calcThermalError() + error;
				moveMotorThermal();
		
	}
Example #10
0
void Motor::moveMotor(int Dir1, int Dir2, int Pwm) {
  if (Dir1 == 1 && Dir2 == 0)
    moveMotor(Pwm);
  if (Dir1 == 0 && Dir2 == 1)
    moveMotor(-Pwm);
}
Example #11
0
void HandInterface::moveMotors(const QList<int> &data)
{
	for (int i = 0; i < HandConsts::numberOfMotors; i++) {
		moveMotor(i, data.at(i));
	}
}