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;
}
}
}
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);
}
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);
}
/*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;
}
/*
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];
}
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;
}
/**
* 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);
}
void Motor_Claire::sendPWMCommand(int command)
{
_speed = min(abs(command), 255);
_direction = (command >= 0) ? FORWARD : BACKWARD;
moveMotor(_speed, _direction);
}
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();
}
void Motor::moveMotor(int Dir1, int Dir2, int Pwm) {
if (Dir1 == 1 && Dir2 == 0)
moveMotor(Pwm);
if (Dir1 == 0 && Dir2 == 1)
moveMotor(-Pwm);
}
void HandInterface::moveMotors(const QList<int> &data)
{
for (int i = 0; i < HandConsts::numberOfMotors; i++) {
moveMotor(i, data.at(i));
}
}
