void AH_28BYJ48::step(int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
if (steps_to_move > 0) {this->direction = 1;}
if (steps_to_move < 0) {this->direction = 0;}
while(steps_left > 0) {
if (millis() - this->last_step_time >= this->step_delay) {
this->last_step_time = millis();
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
steps_left--;
stepMotor(this->step_number % 8);
}
}
}
void feedPaper(){
while (!paperSense()){
feed.right();
}
stepMotor(feedEnc, feed, 5500);
feedEnc.zero();
}
int AH_28BYJ48::step_noblock(int do_steps)
{
static int steps_left = 0;
if (0 == steps_left) {
steps_left = abs(do_steps);
this->direction = (do_steps > 0) ? 1 : 0;
}
if (micros() - this->last_step_us >= this->step_delay_us) {
this->last_step_us = micros();
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
steps_left--;
stepMotor(this->step_number % 8);
}
return steps_left;
}
float collectDiscreteFourierData(FILE* fp, int* photoDetector, int numPhotoDetectors,int motor, int revolutions)
{
float sumSin=0;
float sumCos=0;
int count=0;
int steps,i,j,k;
float angle;
float laserAngle=-100;
int nSamples=16;
float* measurement = malloc(nSamples*sizeof(float));
float* involts = calloc(numPhotoDetectors,sizeof(float));
float* stdDev = calloc(numPhotoDetectors,sizeof(float));
for (k=0;k<revolutions;k++){ //revolutions
for (steps=0;steps < STEPSPERREV;steps+=(int)STEPSIZE){ // steps
// (STEPSPERREV) in increments of STEPSIZE
delay(300); // watching the o-scope, it looks like it takes ~100ms for the ammeter to settle after a change in LP. UPDATE: with the Lock-in at a time scale of 100 ms, it takes 500 ms to settle.
// With time scale of 30 ms, takes 300 ms to settle.
//get samples and average
for(j=0;j<numPhotoDetectors;j++){ // numPhotoDet1
involts[j]=0.0;
for (i=0;i<nSamples;i++){ // nSamples
getUSB1208AnalogIn(photoDetector[j],&measurement[i]);
involts[j]=involts[j]+fabs(measurement[i]);
delay(WAITTIME);
} // nSamples
involts[j]=involts[j]/(float)nSamples;
stdDev[j]=stdDeviation(measurement,nSamples);
} // numPhotoDet1
fprintf(fp,"%d\t",steps+(int)STEPSPERREV*k);
for(j=0;j<numPhotoDetectors;j++){
if(j!=numPhotoDetectors-1)
fprintf(fp,"%f\t%f\t",involts[j],stdDev[j]);
else
fprintf(fp,"%f\t%f\n",involts[j],stdDev[j]);
}
angle=2.0*PI*(steps)/STEPSPERREV;
sumSin+=involts[1]*sin(2*angle);
sumCos+=involts[1]*cos(2*angle);
count++;
stepMotor(motor,CLK,(int)STEPSIZE);
} // steps
laserAngle=180/PI*0.5*atan2(sumCos,sumSin);
} // revolutions
free(measurement);
free(stdDev);
free(involts);
return laserAngle;
}
/*
* Moves the motor steps_to_move steps. If the number is negative,
* the motor moves in the reverse direction.
*/
void Stepper::step(int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (steps_to_move > 0) { this->direction = 1; }
if (steps_to_move < 0) { this->direction = 0; }
// decrement the number of steps, moving one step each time:
while (steps_left > 0)
{
unsigned long now = micros();
// move only if the appropriate delay has passed:
if (now - this->last_step_time >= this->step_delay)
{
// get the timeStamp of when you stepped:
this->last_step_time = now;
// increment or decrement the step number,
// depending on direction:
if (this->direction == 1)
{
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else
{
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, ..., {3 or 10}
// if (this->pin_count == 5)
// stepMotor(this->step_number % 10);
// else
stepMotor(this->step_number % 8);
}
}
stepMotor(8);
}
void StepperDriver::maybeStepMotor(long *steps_left_ptr){
if (micros() - this->last_step_time >= this->step_delay) {
// get the timeStamp of when you stepped:
this->last_step_time = micros();
*steps_left_ptr = *steps_left_ptr - 1L;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->direction);
}
}
void MotorDriver::step(int steps_to_move) {
int steps_left = abs(steps_to_move); // how many steps to take
if (steps_to_move > 0) {
this->direction = 1;
}
if (steps_to_move < 0) {
this->direction = 0;
}
while (steps_left > 0) {
if (millis() - this->last_step_time >= this->step_delay) {
this->last_step_time = millis();
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
} else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
steps_left--;
switch (phases) {
default:
case 1:
stepMotorOnePhase(this->step_number);
break;
case 2:
stepMotorTwoPhase(this->step_number);
break;
case 3:
stepMotor(this->step_number);
break;
}
}
}
}
void collectDiscreteFourierData(FILE* fp, int* photoDetector, int numPhotoDetectors,int motor, int revolutions)
{
int count=0;
int steps,i,j,k;
int nSamples=16;
float* measurement = malloc(nSamples*sizeof(float));
float* involts = calloc(numPhotoDetectors,sizeof(float));
float* stdDev = calloc(numPhotoDetectors,sizeof(float));
for (k=0;k<revolutions;k++){ //revolutions
for (steps=0;steps < NUMSTEPS;steps+=STEPSIZE){ // steps
// (NUMSTEPS) in increments of STEPSIZE
delay(150); // watching the o-scope, it looks like it takes ~100ms for the ammeter to settle after a change in LP
//get samples and average
for(j=0;j<numPhotoDetectors;j++){ // numPhotoDet1
involts[j]=0.0;
for (i=0;i<nSamples;i++){ // nSamples
getMCPAnalogIn(photoDetector[j],&measurement[i]);
involts[j]=involts[j]+measurement[i];
delay(WAITTIME);
} // nSamples
involts[j]=involts[j]/(float)nSamples;
stdDev[j]=stdDeviation(measurement,nSamples);
} // numPhotoDet1
fprintf(fp,"%d\t",steps+NUMSTEPS*k);
for(j=0;j<numPhotoDetectors;j++){
if(j!=numPhotoDetectors-1)
fprintf(fp,"%f\t%f\t",involts[j],stdDev[j]);
else
fprintf(fp,"%f\t%f\n",involts[j],stdDev[j]);
}
count++;
stepMotor(motor,CLK,STEPSIZE);
} // steps
} // revolutions
free(measurement);
free(stdDev);
free(involts);
}
/**
* Update the step position.
* @private
*/
void Stepper::updateStepPosition() {
// increment or decrement the step number,
// depending on direction:
if (_direction == Stepper::CW) {
_step_number++;
if (_step_number >= _steps_per_rev) {
_step_number = 0;
}
}
else {
if (_step_number <= 0) {
_step_number = _steps_per_rev;
}
_step_number--;
}
// step the motor to step number 0, 1, 2, or 3:
stepMotor(_step_number % 4, _direction);
}
/*
Moves the motor steps_to_move steps. If the number is negative,
the motor moves in the reverse direction.
*/
void NStepper::step(int stepsToMove)
{
if (stepsToMove==0) return;
unsigned long time;
int steps_left = abs(stepsToMove); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (stepsToMove > 0) direction_ = 1;
if (stepsToMove < 0) direction_ = 0;
// decrement the number of steps, moving one step each time:
while (steps_left > 0) {
time = millis();
// move only if the appropriate delay has passed:
if (time - lastTime_ >= stepDelay_) {
// get the timeStamp of when you stepped:
lastTime_ = time;
if (direction_==1) {
stepNumber_++;
if (stepNumber_ == numberOfStepsPerTurn_) {
stepNumber_ = 0;
}
} else {
if (stepNumber_==0) {
stepNumber_ = numberOfStepsPerTurn_;
}
stepNumber_--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(stepNumber_ % 4);
}
}
}
/*
Moves the motor steps_to_move steps. If the number is negative,
the motor moves in the reverse direction.
*/
void Stepper::step(int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (steps_to_move > 0) {
this->direction = 1;
}
if (steps_to_move < 0) {
this->direction = 0;
}
// decrement the number of steps, moving one step each time:
while(steps_left > 0) {
// move only if the appropriate delay has passed:
if (millis() - this->last_step_time >= this->step_delay) {
// get the timeStamp of when you stepped:
this->last_step_time = millis();
// increment or decrement the step number,
// depending on direction:
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->step_number % 4);
}
}
}
/*
Moves the motor steps_to_move steps. If the number is negative,
the motor moves in the reverse direction.
*/
void TwigMotor::step(unsigned char motor_addr, int steps_to_move, int direction)
{
// switch to actual address ...
this->address = motor_addr;
this->direction = direction;
setPWM(255);
int steps_left = abs(steps_to_move); // how many steps to take
// decrement the number of steps, moving one step each time:
while(steps_left > 0) {
// move only if the appropriate delay has passed:
if (millis() - this->last_step_time >= this->step_delay) {
// get the timeStamp of when you stepped:
this->last_step_time = millis();
// increment or decrement the step number,
// depending on direction:
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->step_number % 4);
}
}
release(this->address);
}
void NStepper::singleStep(int direction)
{
if (direction==0) return;
if (direction > 0) direction_ = 1;
if (direction < 0) direction_ = 0;
if (direction_==1) {
stepNumber_++;
if (stepNumber_ == numberOfStepsPerTurn_) {
stepNumber_ = 0;
}
} else {
if (stepNumber_==0) {
stepNumber_ = numberOfStepsPerTurn_;
}
stepNumber_--;
}
// step the motor to step number 0, 1, 2, or 3:
stepMotor(stepNumber_ % 4);
lastTime_ = millis();
}
/*
Moves the motor steps_to_move steps. If the number is negative,
the motor moves in the reverse direction.
*/
void StepperMod::step(int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (steps_to_move > 0) {this->direction = 1;}
if (steps_to_move < 0) {this->direction = 0;}
// Turn on enablers
digitalWrite(enablerA_pin, HIGH);
digitalWrite(enablerB_pin, HIGH);
// decrement the number of steps, moving one step each time:
while(steps_left > 0) {
// move only if the appropriate delay has passed:
// increment or decrement the step number,
// depending on direction:
if (this->direction == 1) {
this->step_number++;
if (this->step_number == this->number_of_steps) {
this->step_number = 0;
}
}
else {
if (this->step_number == 0) {
this->step_number = this->number_of_steps;
}
this->step_number--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->step_number % 4);
}
}
void stepperStep(int32_t steps)
{
uint32_t stepsLeft = abs(steps); // Force number to be positive
while (stepsLeft > 0)
{
// Wait 1 tick between individual steps
timer32Delay(0, 1);
// Increment or decrement step counters (depending on direction)
if (steps > 0)
{
stepperPosition++; // Increment global position counter
stepperStepNumber++; // Increment single rotation counter
if (stepperStepNumber == stepperStepsPerRotation)
{
stepperStepNumber = 0;
}
}
else
{
stepperPosition--; // Decrement global position counter
if (stepperStepNumber == 0)
{
stepperStepNumber = stepperStepsPerRotation;
}
stepperStepNumber--; // Decrement single rotation counter
}
// Decrement number of remaining steps
stepsLeft--;
// Step the motor one step
stepMotor(stepperStepNumber % 4);
}
}
/*
* Moves the motor target position. If the number is negative, the motor moves in the reverse direction.
* blocking function - allow target change.
*/
void moveToTargetPos(stepper_struct* current_stepper)
{
int32_t steps_to_move = current_stepper->target_step_number - current_stepper->current_step_number;
while (steps_to_move != 0){
if(steps_to_move >= 0){
steps_to_move += current_stepper->correction_pulses;
}
else{
steps_to_move -= current_stepper->correction_pulses;
}
if (steps_to_move > 0) // determine direction based on whether steps_to_move is + or -:
{
current_stepper->direction = DIRECTION_CW;
current_stepper->current_step_number++; // increment overall step number - from home position
current_stepper->step_number++; // increment motor current step number (1-number_of_steps)
if( (current_stepper->step_number) == current_stepper->number_of_steps) {
current_stepper->step_number = 0;
}
if (current_stepper->use_half_step){
stepMotor(current_stepper, current_stepper->step_number % 8); // step the motor to step number 0, 1, 2, 3, 4, 5, 6 or 7
}
else{
stepMotor(current_stepper, current_stepper->step_number % 4); // step the motor to step number 0, 1, 2, or 3
}
}
else{ //direction = DIRECTION_CCW
current_stepper->direction = DIRECTION_CCW;
current_stepper->current_step_number--; // increment overall step number - from home position
if (current_stepper->step_number == 0) {
current_stepper->step_number = current_stepper->number_of_steps -1;
}
else{
current_stepper->step_number--; // increment motor current step number (1-number_of_steps)
}
if (current_stepper->use_half_step){
stepMotor(current_stepper, current_stepper->step_number % 8); // step the motor to step number 0, 1, 2, 3, 4, 5, 6 or 7
}
else{
stepMotor(current_stepper, current_stepper->step_number % 4); // step the motor to step number 0, 1, 2, or 3
}
}
// create step delay
TIM3_update_flag = 0;
TIM_SetAutoreload(TIM3, current_stepper->stepper_speed); //set period
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
// update steps to move(in the middle of this function some interrupt my change target position)
steps_to_move = current_stepper->target_step_number - current_stepper->current_step_number;
} // end of while loop - step to target.
if (current_stepper->maintain_position == MAINTAIN_POS){ //add aditional step delay, before pins are reseted
TIM3_update_flag = 0;
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
}
//reset motor pins
GPIO_ResetBits(current_stepper->motor_pin_1_bank, current_stepper->motor_pin_1);
GPIO_ResetBits(current_stepper->motor_pin_2_bank, current_stepper->motor_pin_2);
if(current_stepper->pin_count == 4){
GPIO_ResetBits(current_stepper->motor_pin_3_bank, current_stepper->motor_pin_3);
GPIO_ResetBits(current_stepper->motor_pin_4_bank, current_stepper->motor_pin_4);
}
}
/*
Moves the motor steps_to_move steps. If the number is negative,
the motor moves in the reverse direction.
*/
void dualStep::step(int Ysteps=0, int Xsteps=0)
{
int Ysteps_left = abs(Ysteps); // how many steps to take
int Xsteps_left = abs(Xsteps); // how many steps to take
// determine direction based on whether steps_to_mode is + or -:
if (Ysteps > 0) {this->Ydirection = 1;}
if (Ysteps < 0) {this->Ydirection = 0;}
if (Xsteps > 0) {this->Xdirection = 1;}
if (Xsteps < 0) {this->Xdirection = 0;}
// decrement the number of steps, moving one step each time:
while(Xsteps_left > 0 || Ysteps_left > 0) {
//block for the Ymotor
if(Ysteps_left > 0) {
if (millis() - this->Ylast_step_time >= this->Ystep_delay) {
// move only if the appropriate delay has passed:
// get the timeStamp of when you stepped:
this->Ylast_step_time = millis();
// increment or decrement the step number,
// depending on direction:
if (this->Ydirection == 1) {
this->Ystep_number++;
if (this->Ystep_number == this->Ynumber_of_steps) {
this->Ystep_number = 0;
}
}
else {
if (this->Ystep_number == 0) {
this->Ystep_number = this->Ynumber_of_steps;
}
this->Ystep_number--;
}
// decrement the steps left:
Ysteps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->Ystep_number % 4, 0);
}
}
//block for the Xmotor
if(Xsteps_left > 0) {
if (millis() - this->Xlast_step_time >= this->Xstep_delay) {
// move only if the appropriate delay has passed:
// get the timeStamp of when you stepped:
this->Xlast_step_time = millis();
// increment or decrement the step number,
// depending on direction:
if (this->Xdirection == 1) {
this->Xstep_number++;
if (this->Xstep_number == this->Xnumber_of_steps) {
this->Xstep_number = 0;
}
}
else {
if (this->Xstep_number == 0) {
this->Xstep_number = this->Xnumber_of_steps;
}
this->Xstep_number--;
}
// decrement the steps left:
Xsteps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor(this->Xstep_number % 4, 1);
}
}
}
}
void step(int steps_to_move){
int steps_left = abs(steps_to_move); // how many steps to take
int stepsTotal = abs(steps_to_move);
// determine direction based on whether steps_to_mode is + or -:
if (steps_to_move > 0) {direction = 1;}
if (steps_to_move < 0) {direction = 0;}
int stepStart = 0;
// decrement the number of steps, moving one step each time:
while(steps_left > 0) {
// move only if the appropriate delay has passed:
step_delay = 50;
if ( stepStart <= 4 ) step_delay = 60L * 1000L / 100 / 15;
else if ( stepStart <= MIN( stepsTotal*.05, 5 ) ) step_delay = 60L * 1000L / 100 / 20;
else if ( stepStart <= MIN (stepsTotal*.1 , 10) ) step_delay = 60L * 1000L / 100 / 30;
else if ( stepStart <= MIN( stepsTotal*.15, 15 ) ) step_delay = 60L * 1000L / 100 / 50;
else if ( stepStart <= MIN( stepsTotal*.2, 20) ) step_delay = 60L * 1000L / 100 / 55;
else if ( stepStart <= MIN ( stepsTotal - 10, stepsTotal*.8) ) step_delay = 60L * 1000L / 100 / 60;
else if ( stepStart <= MIN( stepsTotal - 5, stepsTotal*.85) ) step_delay = 60L * 1000L / 100 / 50;
else if ( stepStart <= MIN ( stepsTotal - 3, stepsTotal*.90) ) step_delay = 60L * 1000L / 100 / 30 ;
else if ( stepStart > MIN( stepsTotal - 1, stepsTotal*.95) ) step_delay = 60L * 1000L / 100 / 15;
if (millis() - last_step_time >= step_delay) {
stepStart++;
// get the timeStamp of when you stepped:
last_step_time = millis();
// increment or decrement the step number,
// depending on direction:
if (direction >= 1) {
step_number++;
if ( step_number == stepsTotal) {
step_number = 0;
}
}
if (direction == 0) {
if ( step_number == 0) {
step_number = stepsTotal;
}
step_number--;
}
// decrement the steps left:
steps_left--;
// step the motor to step number 0, 1, 2, or 3:
stepMotor( step_number % 4);
}
}
}
int main() {
UCSR0B = 0;
Serial.begin(115200);
PCMSK0 = (1 << 1) | (1 << 6) | (1 << 2) | (1 << 7);
PCICR |= (1 << PCIE0);
PAPER_SENSOR_PORT |= (1 << PAPER_SENSOR_PIN);
OCR1A = 100;
TCCR1A = 0;
TCCR1B = ((1 << CS11)); // Enable timer, 1mhz
TIMSK1 = (1 << TOIE1) | (1 << OCIE1A);
TCNT1 = 0;
SERVO_DDR |= (1 << SERVO_PIN);
sei();
uint16_t val = 0;
for(;;){
if (carriageEnc.changeFlag || feedEnc.changeFlag){
carriageEnc.changeFlag = feedEnc.changeFlag = 0;
//Serial.println(carriageEnc.speed);
}
if (Serial.available() > 0) {
int incomingByte = Serial.read();
switch (incomingByte){
case 'p':
feedPaper();
break;
case 't':
Serial.println(val);
break;
case 'f':
stepMotor(feedEnc, feed, val);
break;
case 'c':
stepMotor(carriageEnc, carriage, val-carriageEnc.pos);
break;
case 's':
servoPos = val;
_delay_ms(150);
break;
case 'x':
feed.right();
_delay_ms(1500);
feed.disable();
case 'n':
servoPos += 10;
break;
case 'm':
servoPos -= 10;
break;
default:
feed.disable();
carriage.disable();
}
if (incomingByte >='1' && incomingByte <='9'){
val = val*10 + (1 + incomingByte - '1');
}else if (incomingByte == '0'){
val = val*10;
}else{
val = 0;
printStatus();
}
}
}
return 0;
}
/*
* Moves the motor steps_to_move steps. If the number is negative, the motor moves in the reverse direction.
* blocking function - doesn't allow target change.
*/
void step(stepper_struct* current_stepper, int steps_to_move)
{
int steps_left = abs(steps_to_move); // how many steps to take
// determine direction based on whether steps_to_move is + or -:
if (steps_to_move > 0) {current_stepper->direction = DIRECTION_CW;}
if (steps_to_move < 0) {current_stepper->direction = DIRECTION_CCW;}
// decrement the number of steps, moving one step each time:
while(steps_left > 0) {
// increment or decrement the step number, depending on direction:
if( current_stepper->direction == DIRECTION_CW) {
current_stepper->current_step_number++; // increment overall step number - from home position
current_stepper->step_number++;
if( (current_stepper->step_number) == current_stepper->number_of_steps) {
current_stepper->step_number = 0;
}
}
else {
current_stepper->current_step_number--; // decrement overall step number - from home position
if (current_stepper->step_number == 0) {
current_stepper->step_number = current_stepper->number_of_steps;
}
current_stepper->step_number--;
}
// decrement the steps left:
steps_left--;
if (current_stepper->use_half_step)
{
// we have 8 steps, next step number will be 0, 1, 2, 3, 4, 5, 6 or 7:// JZ 20091104
stepMotor(current_stepper, current_stepper->step_number % 8); // JZ 20091104
} else {
// step the motor to step number 0, 1, 2, or 3:
stepMotor(current_stepper, current_stepper->step_number % 4);
}
// create step delay
TIM3_update_flag = 0;
TIM_SetAutoreload(TIM3, current_stepper->stepper_speed); //set period
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
}
if (current_stepper->maintain_position == MAINTAIN_POS){ //add aditional step delay, before pins are reseted
TIM3_update_flag = 0;
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
}
//reset motor pins
GPIO_ResetBits(current_stepper->motor_pin_1_bank, current_stepper->motor_pin_1);
GPIO_ResetBits(current_stepper->motor_pin_2_bank, current_stepper->motor_pin_2);
if(current_stepper->pin_count == 4){
GPIO_ResetBits(current_stepper->motor_pin_3_bank, current_stepper->motor_pin_3);
GPIO_ResetBits(current_stepper->motor_pin_4_bank, current_stepper->motor_pin_4);
}
}
int getPolarizationData(char* fileName, int VHe, int dwell, float leakageCurrent){
char command[64];
char echoData[128];
// Variables for stepper motor control.
int nsteps,steps,ninc,i;
// Variables for data collections.
long returnCounts;
long sumCounts;
float current1, current2,angle;
float currentErr;
float* measurement = calloc(dwell*2,sizeof(float));
// Write Aout for He traget here
setUSB1208AnalogOut(HETARGET,(__u16)VHe);
//i = setSorensen120Volts(VHe,SORENSEN120,GPIBBRIDGE1);
//if(i!=0){
// printf("Error setting Sorensen. Code: %d\n",i);
//}
// NOTE THAT THIS SETS THE FINAL ELECTRON ENERGY. THIS ALSO DEPENDS ON BIAS AND TARGET OFFSET. AN EXCIATION FN WILL TELL THE
// USER WHAT OUT TO USE, OR JUST MANUALLY SET THE TARGET OFFSET FOR THE DESIRED ENERGY
// Begin File setup
FILE* rawData=fopen(fileName,"a");
if (!rawData) {
printf("Unable to open file: %s\n",fileName);
exit(1);
}
// End File setup
homeMotor(POL_MOTOR);
nsteps=STEPSPERREV*REVOLUTIONS;
ninc=STEPSPERREV/DATAPOINTSPERREV; // The number of steps to take between readings.
fprintf(rawData,"STEP\tCOUNT\tCURRENT\tCURRENTsd\tANGLE\n");// This line withough a comment is vital for being able to quickly process data. DON'T REMOVE
printf("Steps\tCounts\tCurrent\n");
for (steps=0;steps<nsteps;steps+=ninc){
stepMotor(POL_MOTOR,CCLK,ninc);
current1=0.0;
current2=0.0;
sumCounts=0;
for(i=0;i<dwell;i++){
//writeRS485to232Bridge("READ?",echoData,0xCA);
//current += atof(echoData);
do{
getUSB1208AnalogIn(0,¤t1);
}while(current1 == 0);
getUSB1208Counter(10,&returnCounts);
sumCounts += returnCounts;
do{
getUSB1208AnalogIn(0,¤t2);
}while(current2 == 0);
}
currentErr=(current1-current2)/(current1+current2);
angle = (float)steps/STEPSPERREV*2.0*PI;
printf("%d\t%ld\t%1.2e\n",steps,sumCounts,current1+leakageCurrent);
fprintf(rawData,"%d\t%ld\t%e\t%f\t%f\n",steps,sumCounts,current1+leakageCurrent,currentErr,angle);
}
fclose(rawData);
// Reset Helium Target Offset back to zero
i = setSorensen120Volts(VHe,SORENSEN120,GPIBBRIDGE1);
if(i!=0){
printf("Error setting Sorensen. Code: %d\n",i);
}
//setUSB1208AnalogOut(HETARGET,0);
return 0;
}
/*
* Blocking move function. Allow change of target position, move optimally
* range: from -((steps_per_revolution/2)-1) to +(steps_per_revolution/2);
*/
void moveToTargetPosOptimally(stepper_struct* current_stepper)
{
int32_t steps_to_move = 0;
int32_t abs_steps_to_move = 0;
while ( current_stepper->target_step_number != current_stepper->current_step_number){
steps_to_move = current_stepper->target_step_number - current_stepper->current_step_number;
abs_steps_to_move = abs(steps_to_move);
if (abs_steps_to_move > (current_stepper->steps_per_revolution/2)){
//steps_to_move = -(STEPS_PER_REVOLUTION - abs_steps_to_move);
if (steps_to_move >= 0) steps_to_move = -(current_stepper->steps_per_revolution - abs_steps_to_move);
else steps_to_move = current_stepper->steps_per_revolution - abs_steps_to_move;
}
if (steps_to_move == 0) break;
if (steps_to_move > 0) // direction = DIRECTION_CW:
{
current_stepper->direction = DIRECTION_CW;
current_stepper->current_step_number++; // increment overall step number - from home position
current_stepper->step_number++; // increment motor current step number (1-number_of_steps)
if( (current_stepper->step_number) == current_stepper->number_of_steps){
current_stepper->step_number = 0;
}
if (current_stepper->use_half_step){
stepMotor(current_stepper, current_stepper->step_number % 8); // step the motor to step number 0, 1, 2, 3, 4, 5, 6 or 7
}
else{
stepMotor(current_stepper, current_stepper->step_number % 4); // step the motor to step number 0, 1, 2, or 3
}
}
else{ //direction = DIRECTION_CCW
current_stepper->direction = DIRECTION_CCW;
current_stepper->current_step_number--; // increment overall step number - from home position
if (current_stepper->step_number == 0){
current_stepper->step_number = current_stepper->number_of_steps -1;
}
else{
current_stepper->step_number--; // increment motor current step number (1-number_of_steps)
}
if (current_stepper->use_half_step){
stepMotor(current_stepper, current_stepper->step_number % 8); // step the motor to step number 0, 1, 2, 3, 4, 5, 6 or 7
}
else{
stepMotor(current_stepper, current_stepper->step_number % 4); // step the motor to step number 0, 1, 2, or 3
}
}
// create step delay
TIM3_update_flag = 0;
TIM_SetAutoreload(TIM3, current_stepper->stepper_speed); //set period
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
if (current_stepper->current_step_number > (current_stepper->steps_per_revolution/2)){
current_stepper->current_step_number = -((current_stepper->steps_per_revolution/2)-1);
}
if (current_stepper->current_step_number < -((current_stepper->steps_per_revolution/2)-1)){
current_stepper->current_step_number = (current_stepper->steps_per_revolution/2);
}
} // end of while loop - step to target.
if (current_stepper->maintain_position == MAINTAIN_POS){ //add aditional step delay, before pins are reseted
TIM3_update_flag = 0;
TIM_SetCounter(TIM3, 0);
TIM_Cmd(TIM3, ENABLE);
while(TIM3_update_flag != 1);
TIM_Cmd(TIM3, DISABLE);
}
//reset motor pins
GPIO_ResetBits(current_stepper->motor_pin_1_bank, current_stepper->motor_pin_1);
GPIO_ResetBits(current_stepper->motor_pin_2_bank, current_stepper->motor_pin_2);
if(current_stepper->pin_count == 4){
GPIO_ResetBits(current_stepper->motor_pin_3_bank, current_stepper->motor_pin_3);
GPIO_ResetBits(current_stepper->motor_pin_4_bank, current_stepper->motor_pin_4);
}
}
#include "Gcode.h"
#include "Mcode.h"
#include "StepMotor.h"
#include "pinDefine.h"
#include "config.h"
stepMotor smX=stepMotor('X');
stepMotor smY=stepMotor('Y');
stepMotor smZ=stepMotor('Z');
stepMotor smE=stepMotor('E');
int targetTemp=60; //default target: 60
boolean heaterOn=false;
Gcode::Gcode(String inString){
code=inString;
}
String Gcode::get(){
return code;
}
void Gcode::solve(){
int readPos =1;//reading postion
String StrCodeNum="";
for(readPos=1;code[readPos]!=' '&&readPos<code.length();readPos++)
{
StrCodeNum= StrCodeNum+code[readPos];
}
int codeNum=atoi(StrCodeNum.c_str());
switch (codeNum){ //The codes are executed differently according to the number after 'G'('g').