void motor_reset() {
/* Begin with resetting axis Z if no reset is already in progress. It is
* imperative to first retract on axis Z separately from the others. Once
* that has been dealt with, axes X and Y may follow. */
if(bit_is_clear(motor_status, MTR_RESET)) {
motor_stop();
motor_status |= _BV(MTR_RESET) | _BV(MTR_IS_Z);
setup_axis(AXIS_Z, MTR_INC);
LOCK_DISABLE();
motor_start();
/* Axis Z has been reset. Now, axes X and Y may follow. */
} else if(bit_is_set(motor_status, MTR_RESET_Z_DONE)) {
/* Remove flag denoting axis Z is resetting. */
motor_status &= ~(_BV(MTR_IS_Z) | _BV(MTR_RESET_Z_DONE));
/* Reset axes X and Y. */
setup_axis(AXIS_Y, MTR_DEC);
setup_axis(AXIS_X, MTR_DEC);
LOCK_DISABLE(); /* Manually enable PWM propagation. */
motor_start();
/* All resetting stages have been completed. Reset #cur_pos and flags. */
} else if(bit_is_set(motor_status, MTR_RESET_X_DONE)
&& bit_is_set(motor_status, MTR_RESET_Y_DONE)) {
cur_pos.x = 0;
cur_pos.y = 0;
cur_pos.z = max_pos.z - 1;
motor_stop();
LOCK_ENABLE();
/* If this resetting cycle was initiated as a response to a limit being
* engaged while under normal motor operation, retry reaching #new_pos
* anew. */
if(bit_is_set(motor_status, MTR_LIMIT)) {
if(motor_update()) {
motor_stop();
MTR_CALL(cur_pos, MTR_EVT_OK);
}
} else {
new_pos = cur_pos;
MTR_CALL(cur_pos, MTR_EVT_OK);
}
motor_status &= ~(_BV(MTR_RESET) | _BV(MTR_LIMIT)
| _BV(MTR_RESET_X_DONE) | _BV(MTR_RESET_Y_DONE));
motor_status |= _BV(MTR_IS_RST_FRESH);
/* Reset is in progress. */
} else {
}
}
void main (void)
{
init_clock();
while(1)
{
switch (get_key())
{
case 0:
motor_start();
break;
case 1:
motor_stop();
break;
case 2:
drill_down();
break;
case 3:
drill_up();
break;
case 4:
step();
break;
case 5:
drill_hole();
break;
case 6:
ref_pos();
break;
case 7:
do_auto(pattern);
break;
}
}
}
void motor_set_speed(int16_t speed)
{
if(speed < 0)
{
motor_direction = DIR_COUNTERCLOCKWISE;
commutation_table = ccw;
}
else if(speed > 0)
{
motor_direction = DIR_CLOCKWISE;
commutation_table = cw;
}
/* Saturation */
if((speed * motor_direction) > 100)
{
motor_speed = (100 * motor_direction);
}
else
{
motor_speed = (speed * motor_direction);
}
/* Starting up motor if speed is zero */
if(!motor_read_speed())
{
motor_start();
}
}
void task_main(void *pvParameters)
{
while(pb_read(pb2) == 0)
{
// wait for button pressed.
}
vTaskDelay(1000);
for (int speed = 1000; speed < 10000; speed = speed + 50)
{
motor_go_forward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_go_backward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_right();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_left();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
}
// pidMotorMoveFor1Cell(85);
while(1)
{
}
}
static int8_t motor_update() {
uint8_t steps = 0;
/* Motion along axes X and Y takes precedence over motion along axis Z, when
* no submersion is requested on the latter. Also, only motion along axes X
* and Y may be combined with one another. */
if((new_pos.x != cur_pos.x || new_pos.y != cur_pos.y)
&& new_pos.z <= cur_pos.z) {
/* Relative offset from #cur_pos on axes X and Y. */
int16_t rel_x = (int16_t)new_pos.x - (int16_t)cur_pos.x;
int16_t rel_y = (int16_t)new_pos.y - (int16_t)cur_pos.y;
/* Enable lines for PWM propagation to motor Y and set its speed. */
if(rel_y) {
setup_axis(AXIS_Y, rel_y); /* @c rel_y is used only for its sign. */
}
/* Enable lines for PWM propagation to motor X and set its speed. Motor
* X may only be set-up after motor Y, as noted in #setup_axis(). */
if(rel_x) {
setup_axis(AXIS_X, rel_x); /* @c rel_x is used only for its sign. */
}
rel_x = abs(rel_x);
rel_y = abs(rel_y);
if(rel_x > 0 && rel_y > 0) {
/* Request the common amount of steps between @c rel_x and
* @c rel_y. */
steps = rel_x >= rel_y ? GRID_TO_STEP(rel_y)
: GRID_TO_STEP(rel_x);
} else {
/* If one of @c rel_x and @c rel_y is @c 0, then calculate the
* amount of steps solely based on the other (non-zero) value. */
steps = rel_x > 0 ? GRID_TO_STEP(rel_x)
: GRID_TO_STEP(rel_y);
}
/* Configure motion along axis Z. */
} else if(new_pos.z != cur_pos.z) {
int16_t rel_z = (int16_t)new_pos.z - cur_pos.z;
setup_axis(AXIS_Z, rel_z); /* @c rel_z is used only for its sign. */
steps = GRID_TO_STEP(abs(rel_z));
motor_status |= _BV(MTR_IS_Z);
} else {
/* Already at #new_pos. */
motor_status &= ~_BV(MTR_IS_Z);
return -1;
}
setup_lock(steps);
motor_start();
return 0;
}
void backward(uint count)
{
if(count)
{
TIMSK|=(1<<7);
ctc_cmp=count;
motor_start();
DDRB|=0x03;
PORTB=~((PORTB|0x02)&(~0x01));
while(ctc_cmp);
TIMSK&=~(1<<7);
motor_stop();
}
else
{
motor_start();
DDRB|=0x03;
PORTB=~((PORTB|0x02)&(~0x01));
}
}
/* This function starts the calibration task */
void calibration_start_task(void)
{
calibration.state = CALIBRATION_INIT;
calibrationTimer.interval = CALIBRATION_FAST_TASK_PERIOD;
calibrationTimer.mode = TIMER_REPEAT_MODE;
calibrationTimer.callback = calibration_task;
calibration_init_structure();
servo_structure_init();
motor_start();
HAL_StartAppTimer(&calibrationTimer);
}
void right(uint count)
{
if(count)
{
TIMSK|=(1<<7);
ctc_cmp=count;
motor_start();
DDRB|=0x03;
PORTB&=~0x03;
while(ctc_cmp);
TIMSK&=~(1<<7);
motor_stop();
}
else
{
motor_start();
DDRB|=0x03;
PORTB&=~0x03;
}
}
/*
* MAIN
*/
int main(void)
{
DDRB = 0x0E; // PB0 = in, PB1 - PB3 out
PORTB |= 0x01; // set pull-up rezistor on PB0 - output
DDRD &= ~0x04; // PD2 = in
PORTD |= 0x04; // set pull-up rezistor on PD2 - input
motor_pwm_init();
ADCSRA = 0xCB;
ADMUX = 0x60; // Vref Vcc
MCUCR |= 0x02; // interupr int0 acts on falling edge
sei(); // global interrupt enable
motor_start(MOTOR_1);
while(1) {
if ((PINB & 0x01) == SENSOR_MODE && int_wait == 0) { /* */
motor_stop(MOTOR_0);
led_pwm_init();
SENSOR_ON;
int_wait = 1;
}
else if ((PINB & 0x01) == !SENSOR_MODE) {
SENSOR_OFF;
LED_PWM_STOP;
motor_start(MOTOR_0);
int_wait = 0;
}
}
return 0;
}
void do_auto(const int *pattern)
{
int *iterator = pattern;
if (!ref_pos())
return;
motor_start();
for (; *iterator != 0xFF; iterator++)
{
n_step(*iterator);
drill_hole();
}
motor_stop();
}
void ss_sampl_H()
{
// Choose sampling method.
//#define SAMP_METHOD_SWING
#define SAMP_METHOD_SWING_S
// SWING mm
#if defined(SAMP_METHOD_SWING)
#define toR2 24+1 //20
#define toR3 12+2 //10
#define toR1 23 //15
#define toL1 23-2 //20
#define toL3 23 //20
#define toL2 12+3 //15
#define toC1 24+3 //20
#elif defined(SAMP_METHOD_SWING_S)
#define toR2 40
#define toR3 20
#define toR1 40
#define toL1 40
#define toL3 40
#define toL2 20
#define toC1 40
#else
#error Choose Sampling Method
#endif
led_off(PIO_LED_ALL);
led_on (PIO_LED_2 | PIO_LED_6 | PIO_LED_3);
// sys_delay(1000);
ss_on();
// show LEDs to find the center of block
int ss_diff;
while( !( get_key_state()==(PORT_CANCEL) || get_key_state()==(PORT_SELECT) ) )
{
ss_diff = ((int)ss_value[SS_L45]) - ((int)ss_value[SS_R45]);
led_off(PIO_LED_ALL);
if ( ss_diff > 10) led_on(PIO_LED_3);
else if ( ss_diff > 5) led_on(PIO_LED_4 | PIO_LED_1);
else if ( ss_diff < -10) led_on(PIO_LED_2);
else if ( ss_diff < -5 ) led_on(PIO_LED_5 | PIO_LED_0);
else led_on(PIO_LED_6);
}
// wait_key();
torque_on();
wait_key();
// ss_on();
led_count_down( 5, 1000 );
// debug
//#define DEBUG_SAMPL
disable_adj();
reset_spd_ptr();
// C1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_6 );
sys_delay(100); sampl_value( sC1 );
motor_start();
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R2
led_off( PIO_LED_0|PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_0|PIO_LED_5|PIO_LED_2 );
#if defined(SAMP_METHOD_SWING)
swing(toR2);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toR2);
#else
#error Choose Sampling Method
#endif
sampl_value( sR2 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R3
led_off( PIO_LED_0|PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_2 );
#if defined(SAMP_METHOD_SWING)
swing(toR3);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toR3);
#else
#error Choose Sampling Method
#endif
sampl_value( sR3 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// R1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_0 | PIO_LED_5);
#if defined(SAMP_METHOD_SWING)
swing(-toR1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toR1);
#else
#error Choose Sampling Method
#endif
sampl_value( sR1 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_1 | PIO_LED_4);
#if defined(SAMP_METHOD_SWING)
swing(-toL1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toL1);
#else
#error Choose Sampling Method
#endif
sampl_value( sL1 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L3
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_3);
#if defined(SAMP_METHOD_SWING)
swing(-toL3);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(-toL3);
#else
#error Choose Sampling Method
#endif
sampl_value( sL3 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// L2
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_1|PIO_LED_3|PIO_LED_4 );
#if defined(SAMP_METHOD_SWING)
swing(toL2);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toL2);
#else
#error Choose Sampling Method
#endif
sampl_value( sL2 );
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
// C1
led_off( PIO_LED_0 |PIO_LED_1|PIO_LED_2|PIO_LED_3|PIO_LED_4|PIO_LED_5|PIO_LED_6);
led_on ( PIO_LED_6 );
#if defined(SAMP_METHOD_SWING)
swing(toC1);
#elif defined(SAMP_METHOD_SWING_S)
swing_S(toC1);
#else
#error Choose Sampling Method
#endif
#ifdef DEBUG_SAMPL
motor_stop(); wait_key(); motor_start();
#endif
motor_stop();
ss_off();
torque_off();
}
void ss_sampl()
{
while( 1 ) {
// Sampling Vertical //////////////////////////////////////
led_off(PIO_LED_ALL);
led_on (PIO_LED_0|PIO_LED_1|PIO_LED_5|PIO_LED_4);
if ( L_KEY == read_key() ) {
Sound_Select();
ss_sampl_V(); process_sampl_V();
save_sampl_value();
} else Sound_Beep();
// Sampling Horizonal //////////////////////////////////////
led_off(PIO_LED_ALL);
led_on (PIO_LED_2|PIO_LED_6|PIO_LED_3);
if ( L_KEY == read_key() ) {
Sound_Select();
ss_sampl_H(); process_sampl_V();
save_sampl_value();
} else Sound_Beep();
}
END_SEL:
}
void ss_sampl_V()
{
led_off(PIO_LED_ALL);
led_on (PIO_LED_0 | PIO_LED_1 | PIO_LED_5 | PIO_LED_4);
wait_key(); torque_on(); wait_key();
led_count_down( 5, 1000 );
disable_adj();
spd_ptr = 0;
spd_diff = 0;
reverse_LR();
ss_on(); sys_delay(300);
unsigned int ref_L;
unsigned int ref_R;
ref_L = ss_value[SS_L0];
ref_R = ss_value[SS_R0];
int i=0;
reset_spd_ptr();
spd_diff = 1;
motor_start();
while(i < SAMPL_VSTEP)
{
#define SAM_SPD 20 //25
if ( spd_ptr_L >= SAM_SPD ) {
spd_diff = 0;
equal_spd_ptr();
}
reset_steps();
while( steps == 0 );
ref_L = ss_value[SS_L0];
ref_R = ss_value[SS_R0];
sFL0[i] = (unsigned char)ref_L;
sFR0[i] = (unsigned char)ref_R;
i++;
}
ss_off();
spd_diff = -1;
while (spd_ptr_L > 0);
reset_spd_ptr();
motor_stop();
sys_delay(200);
reverse_LR();
torque_off();
}
void terminate(void){ // terminate function is called 2 seconds after bluetooth is disconnected
for(int index = 0;index<6;index++){
motor_start(index, 0);
}
}
void cmdFuncStart(char *param)
{
// printf("Hello, I'm help function %s\r\n", ":)");
motor_start(&(motor[M2_IDX]));
}
