void test_relais(void) {
bit_clr(PORTD, P_BIT4);
bit_clr(PORTD, P_BIT5);
bit_clr(PORTD, P_BIT6);
bit_clr(PORTD, P_BIT7);
bit_set(PORTD, WIND_CLOCK);
uart_puts ("0x80\r\n");
lcd_puts (0, "bit7 (wind-clock)\n");
delay(2000);
bit_set(PORTD, MOTOR);
uart_puts ("0x40\r\n");
lcd_puts (0, "bit5 (motor)\n");
delay(2000);
bit_set(PORTD, FILL_WIND);
uart_puts ("0x20\r\n");
lcd_puts (0, "bit6 (fill-wind)\n");
delay(2000);
bit_set(PORTD, WIND_ANTICLOCK);
uart_puts ("0x10\r\n");
lcd_puts (0, "bit4 (wind anticlock)\n");
delay(2000);
lcd_clrscr(0);
uart_puts ("mot\r\n");
lcd_puts (0, "motor\n");
start_motor();
delay(2000);
stop_motor();
delay(500);
uart_puts ("fill\r\n");
lcd_puts (0, "fill\n");
fill();
delay(2000);
stop_motor();
uart_puts ("windleft\r\n");
lcd_puts (0, "windleft\n");
delay(500);
set_wind_dir_left();
delay(5000);
stop_motor();
}
/**
* Rotate motors X,Y to direction dir (CW > 0)
* Stop motor if dir == 0
*/
void move_motor(int dir){
if(dir == 0){ // stop
#ifdef __arm__
glob_dir = 0;
stopat = 0;
stop_motor();
// exit(0);
#else // __arm__
printf("Stop motor\n");
#endif // __arm__
}else{
#ifdef __arm__
if((digitalRead(ESW1_PIN) == 0 && dir == -1) ||
(digitalRead(ESW2_PIN) == 0 && dir == 1)){
DBG("already on ESW");
// exit(0);
glob_dir = 0;
}else{
glob_dir = dir;
if(dir > 0) steppart = 0;
else steppart = 7;
DBG("move to %d", dir);
}
#else // __arm__
printf("Move X to %s\n", (dir > 0) ? "++" : "--");
#endif // __arm__
}
}
void signals(int sig){
if(sig > 0)
WARNX(_("Get signal %d, quit.\n"), sig);
unlink(PIDFILE);
stop_motor();
exit(sig);
}
// this function is called from an ISR, so no need to turn off/on interrupts
void check_dispense_complete_isr(void)
{
if (g_dispense_target_ticks > 0 && g_ticks >= g_dispense_target_ticks)
{
g_dispense_target_ticks = 0;
g_is_dispensing = 0;
stop_motor();
adc_shutdown();
}
}
void run_motor_timed(uint32_t duration)
{
uint32_t t;
if (duration == 0)
return;
set_motor_speed(255, 1);
for(t = 0; t < duration && !check_reset(); t++)
_delay_ms(1);
stop_motor();
}
void sysMotorset(int motor,int sens)
{
#ifdef VSIMU
set_motor_dir(motor,sens);
#endif
#ifdef VREAL
// char buffer[256];
motor=1+(motor&1);
// sprintf(buffer,"setmotor %d sens %d\r\n",motor,sens);
// consolestr(buffer);
if (sens==0) stop_motor(motor);
else run_motor(motor,255,(sens>0)?REVERSE:FORWARD/*:REVERSE*/);
#endif
}
void setup_pins(){
#ifdef __arm__
wiringPiSetupGpio();
pinMode(MOTOR_PIN1, OUTPUT);
pinMode(MOTOR_PIN2, OUTPUT);
pinMode(MOTOR_PIN3, OUTPUT);
pinMode(MOTOR_PIN4, OUTPUT);
pinMode(LAMP1_PIN, OUTPUT);
pinMode(LAMP2_PIN, OUTPUT);
pinMode(ESW1_PIN, INPUT);
pinMode(ESW2_PIN, INPUT);
stop_motor();
Write(LAMP1_PIN, 1);
Write(LAMP2_PIN, 1);
#else // __arm__
printf("Setup GPIO\n");
#endif // __arm__
}
int flpy_read(struct fdd *d, uint32_t lba, uint8_t *buf, uint32_t nr_sectors)
{
int rc = 0;
uint8_t retries;
struct chs f_addr;
if (d->param->cmos_type == 0) return -1;
while (_busy) ; /* Wait while the floppy driver is already busy. BUSY WAIT! */
_busy = TRUE;
start_motor(d);
specify(d);
/* Only retry for 3 times */
for (retries = 0; retries < 3; retries++) {
/* Move head to right track */
if (flpy_seek(d, f_addr.c) == 0) {
/* If changeline is active, no disk is in drive */
if (inportb(d->fdc->base_port + FDC_DIR) & 0x80) {
DEBUG(DL_ERR, ("no disk in drive %d\n",
d->number));
rc = -1;
goto errorout;
}
rc = fdc_xfer(d, &f_addr, dma_addr, nr_sectors, FDC_READ);
if (rc == 0) break;
} else
rc = -1;
if (retries < 2) recalibrate(d);
}
/* Copy data from the DMA buffer into the caller's buffer */
if ((rc == 0) && buf)
;
errorout:
stop_motor(d);
_busy = FALSE;
return rc;
}
void stop_winding (void)
{
char buf[8];
stop_motor();
update_command();
if (action==INACTIVE) return; // als we wikkelen zonder power aan op relays
if (turns_done==0) {
action=INACTIVE;
return;
}
if (action==WIND_LEFT) { strcat (layer,"L");}
if (action==WIND_RIGHT) { strcat (layer,"R");}
if (action==FILL) { strcat (layer,"F");}
action=INACTIVE;
itoa(turns_done,buf,10);
strcat(layer,buf);
update_layer();
}
int flpy_write(struct fdd *d, uint32_t lba, const uint8_t buf, uint32_t nr_sectors)
{
int rc = 0;
uint8_t retries;
struct chs f_addr;
if (d->param->cmos_type == 0) return -1;
while (_busy) ; // The BUSY WAIT!
_busy = TRUE;
start_motor(d);
specify(d);
for (retries = 0; retries < 3; retries++) {
/* Move head to right track */
if (flpy_seek(d, f_addr.c) == 0) {
/* If changeline is active, no disk is in drive */
if (inportb(d->fdc->base_port + FDC_DIR) & 0x80) {
DEBUG(DL_ERR, ("no disk in drive %d\n",
d->number));
rc = -1;
break;
}
rc = fdc_xfer(d, &f_addr, dma_addr, nr_sectors, FDC_WRITE);
if (rc == 0) break;
} else
rc = -1;
if (retries < 2 ) recalibrate(d);
}
stop_motor(d);
_busy = FALSE;
return rc;
}
int main(void)
{
uint8_t id, rec, i, cs;
color_t c;
packet_t p;
setup();
stop_motor();
sei();
for(i = 0; i < 5; i++)
{
set_led_rgb(255, 0, 255);
_delay_ms(50);
set_led_rgb(255, 255, 0);
_delay_ms(50);
}
// get the current liquid level
update_liquid_level();
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
g_current_sense_num_cycles = 0;
setup();
serial_init();
stop_motor();
set_led_rgb(0, 0, 255);
sei();
id = address_exchange();
for(; !check_reset();)
{
rec = receive_packet(&p);
if (rec == COMM_CRC_FAIL)
continue;
if (rec == COMM_RESET)
break;
if (rec == COMM_OK && (p.dest == DEST_BROADCAST || p.dest == id))
{
// If we've detected a over current sitatuion, ignore all comamnds until reset
cli();
cs = g_current_sense_detected;
sei();
switch(p.type)
{
case PACKET_PING:
break;
case PACKET_SET_MOTOR_SPEED:
if (!cs)
set_motor_speed(p.p.uint8[0], p.p.uint8[1]);
if (p.p.uint8[0] == 0)
flush_saved_tick_count(0);
break;
case PACKET_TICK_DISPENSE:
if (!cs)
{
dispense_ticks((uint16_t)p.p.uint32, 255);
flush_saved_tick_count(0);
}
break;
case PACKET_TIME_DISPENSE:
if (!cs)
{
run_motor_timed(p.p.uint32);
flush_saved_tick_count(0);
}
break;
case PACKET_IS_DISPENSING:
is_dispensing();
break;
case PACKET_LIQUID_LEVEL:
get_liquid_level();
break;
case PACKET_UPDATE_LIQUID_LEVEL:
update_liquid_level();
break;
case PACKET_LED_OFF:
set_led_pattern(LED_PATTERN_OFF);
break;
case PACKET_LED_IDLE:
if (!cs)
set_led_pattern(LED_PATTERN_IDLE);
break;
case PACKET_LED_DISPENSE:
if (!cs)
set_led_pattern(LED_PATTERN_DISPENSE);
break;
case PACKET_LED_DRINK_DONE:
if (!cs)
set_led_pattern(LED_PATTERN_DRINK_DONE);
break;
case PACKET_LED_CLEAN:
if (!cs)
set_led_pattern(LED_PATTERN_CLEAN);
break;
case PACKET_COMM_TEST:
comm_test();
break;
case PACKET_ID_CONFLICT:
id_conflict();
break;
case PACKET_SET_CS_THRESHOLD:
g_current_sense_threshold = p.p.uint16[0];
break;
case PACKET_SAVED_TICK_COUNT:
get_saved_tick_count();
break;
case PACKET_RESET_SAVED_TICK_COUNT:
reset_saved_tick_count();
break;
case PACKET_FLUSH_SAVED_TICK_COUNT:
flush_saved_tick_count(1);
break;
case PACKET_GET_LIQUID_THRESHOLDS:
get_liquid_thresholds();
break;
case PACKET_SET_LIQUID_THRESHOLDS:
set_liquid_thresholds(p.p.uint16[0], p.p.uint16[1]);
break;
case PACKET_TICK_SPEED_DISPENSE:
if (!cs)
{
dispense_ticks(p.p.uint16[0], (uint8_t)p.p.uint16[1]);
flush_saved_tick_count(0);
}
break;
case PACKET_PATTERN_DEFINE:
pattern_define(p.p.uint8[0]);
break;
case PACKET_PATTERN_ADD_SEGMENT:
c.red = p.p.uint8[0];
c.green = p.p.uint8[1];
c.blue = p.p.uint8[2];
pattern_add_segment(&c, p.p.uint8[3]);
break;
case PACKET_PATTERN_FINISH:
pattern_finish();
break;
}
}
}
}
return 0;
}
int main(void)
{
uint8_t i;
#ifdef DEBUG
uart_init();
uart_puts ("\r\nreset\r\ninit port 1\r\n");
#endif
//
/*
uart_puts ("a123\n");
uart_puts ("b456\n");
uart_puts ("c789\n");
uart_puts ("d20\n");
*/
init_keypad_4x4s (P_PORTC);
init_count_switch();
init_timer ();
lcd_setup (0, P_PA2, P_PA4, P_PA6, P_PA0,
P_PA1, P_PA3, P_PA5, P_PA7);
//lcd_setup (uint8_t chip, uint8_t strobe, uint8_t clock, uint8_t io)
lcd_setup_info (0, HD44780, 20, 2);
lcd_init (0, LCD_DISP_ON);
// relays
// sh_cp:11 (geel) st_cp:12(paars) ds:14(wit)
//void hc595_setup (uint8_t chip, uint8_t clk, uint8_t cs, uint8_t data)
DDRD|=P_BIT7 | P_BIT6 | P_BIT5 | P_BIT4;
#ifdef DEBUG
test_relais();
#endif
c=0;
prevc=0;
num=0;
turns_done=0;
turns_prog=0;
total_turns_done=0;
for (i=0; i<80; i++) layer[i]=0;
stop_motor();
set_wind_dir_none();
lcd_clrscr(0);
lcd_puts (0,"* Winding controller 20140114");
lcd_gotoxy(0,39,0);
lcd_puts (0,"*");
lcd_gotoxy(0,0,3);
lcd_puts (0,"*");
lcd_gotoxy(0,39,3);
lcd_puts (0,"*");
lcd_gotoxy(0,2,2);
lcd_printf(0,"Free RAM:%d b",freeRam());
sleep(1);
lcd_clrscr(0);
update_info ();
update_command();
update_num ();
update_speed ();
update_turns();
for (;;) {
c=keypad_w_getch();
if (c!=prevc) {
/*a=PINC;
uart_printf ("%x %x %d\r\n",a,c,num); */
prevc=c;
if ((action==FILL) || (action==WIND_LEFT) || (action==WIND_RIGHT)) {
switch (c) {
case 0x0c: stop_winding();
action=INACTIVE; //dubbel maar extra voor readability
break;
}
} // if !INACTIVE
if (action==HELP) {
switch (c) {
case 0x0f: if (helppage<MAX_HELPPAGES) helppage++;
show_help();
break;
case 0x0e: if (helppage>0) helppage--;
show_help();
break;
case 0x0c: action=INACTIVE;
lcd_clrscr(0);
break;
}
} // if HELP
if (action==CORR) {
switch (c) { //corrmode is wat we de *vorige* keer gedaan hebben
char buf[10];
case 0x0c: turns_done=turns_before_corr;
total_turns_done=total_turns_done_before_corr;
action=INACTIVE;
break;
case 0x0e: turns_done=turns_before_corr;
total_turns_done=total_turns_done_before_corr;
if (corrmode==SUB) {
turns_done+=corr;
total_turns_done+=corr;
corrmode=ADD;
break;
}
if (corrmode==ADD) {
if (turns_done>=corr) {
turns_done-=corr;
total_turns_done-=corr;
}
corrmode=SUB;
}
break;
case 0x0f: turns_done=0;
total_turns_done=0;
turns_before_corr=0;
total_turns_done_before_corr=0;
action=INACTIVE;
break;
case 0xff:
case 0xfd:
case 0xfe: break;
default:
num=0;
action=INACTIVE;
itoa(corr,buf,10);
if (corrmode==SUB) strcat(layer,"+");
if (corrmode==ADD) strcat(layer,"+");
strcat (layer,buf);
update_layer();
break;
}
update_command();
update_turns();
}
if (action==INACTIVE) {
switch (c) {
case 0x0a: wind_left();
turns_prog=num;
num=0;
break;
case 0x0b: wind_right ();
turns_prog=num;
num=0;
break;
case 0x0d: fill ();
turns_prog=num;
num=0;
break;
case 0x0c: num=0;
stop_motor();
break;
case 0x0e: corr=num;
turns_before_corr=turns_done;
if (turns_done>=corr) turns_done-=corr;
action=CORR;
corrmode=SUB;
break;
case 0x0f: helppage=0;
show_help();
action=HELP;
break;
case 0xff:
case 0xfd:
case 0xfe: break;
default: if (num<1000) {
num=num*10 + c;
update_num();
update_speed();
}
}
if (action!=HELP) {
update_command();
update_turns();
update_num();
update_speed();
}
} // if INACTIVE
} // if c==prevc
} // mainloop
return 0;
}
int main() {
unsigned long T = 0L;
int Ival;
U8 rb;
CFG_GCR |= 1; // disable SWIM
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Configure timer 1 - systick
// prescaler = f_{in}/f_{tim1} - 1
// set Timer1 to 1MHz: 1/1 - 1 = 15
TIM1_PSCRH = 0;
TIM1_PSCRL = 15; // LSB should be written last as it updates prescaler
// auto-reload each 1ms: TIM_ARR = 1000 = 0x03E8
TIM1_ARRH = 0x03;
TIM1_ARRL = 0xE8;
// interrupts: update
TIM1_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM1_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
// Configure pins
// PC2 - PP output (on-board LED)
PORT(LED_PORT, DDR) |= LED_PIN;
PORT(LED_PORT, CR1) |= LED_PIN;
// PD5 - UART2_TX
PORT(UART_PORT, DDR) |= UART_TX_PIN;
PORT(UART_PORT, CR1) |= UART_TX_PIN;
// Configure UART
// 8 bit, no parity, 1 stop (UART_CR1/3 = 0 - reset value)
// 57600 on 16MHz: BRR1=0x11, BRR2=0x06
UART2_BRR1 = 0x11; UART2_BRR2 = 0x06;
UART2_CR2 = UART_CR2_TEN | UART_CR2_REN | UART_CR2_RIEN; // Allow RX/TX, generate ints on rx
// enable all interrupts
enableInterrupts();
set_stepper_speed(1000);
setup_stepper_pins();
// Loop
do{
if((Global_time - T > paused_val) || (T > Global_time)){
T = Global_time;
PORT(LED_PORT, ODR) ^= LED_PIN; // blink on-board LED
}
if(UART_read_byte(&rb)){ // buffer isn't empty
switch(rb){
case 'h': // help
case 'H':
uart_write("\nPROTO:\n+/-\tLED period\nS/s\tset/get Mspeed\n"
"m\tget steps\nx\tstop\np\tpause/resume\nM\tmove motor\na\tadd Nstps\n"
"u\tunipolar motor\nb\tbipolar motor\n");
break;
case '+':
paused_val += 100;
if(paused_val > 10000)
paused_val = 500; // but not more than 10s
break;
case '-':
paused_val -= 100;
if(paused_val < 100) // but not less than 0.1s
paused_val = 500;
break;
case 'S': // set stepper speed
if(readInt(&Ival) && Ival > MIN_STEP_LENGTH)
set_stepper_speed(Ival);
else
error_msg("bad speed");
break;
case 's': // get stepper speed
printUint((U8*)&Stepper_speed, 2);
break;
case 'm': // how much steps there is to the end of moving
printUint((U8*)&Nsteps, 4);
break;
case 'M': // move motor
if(Nsteps){
error_msg("moving!");
break;
}
if(readInt(&Ival) && Ival)
move_motor(Ival);
else{
error_msg("bad Nsteps");
}
break;
case 'x': // stop
stop_motor();
break;
case 'p': // pause/resume
pause_resume();
break;
case 'a': // add N steps
if(readInt(&Ival) && Ival){
add_steps(Ival);
}else{
error_msg("bad value");
}
break;
case 'u': // unipolar
usteps = ustepsUNI;
break;
case 'b': // bipolar
usteps = ustepsBIP;
break;
}
}
}while(1);
}
/**
* Check flags set by timers & do next:
* - decrease step counter if it isn't zero;
* - stop motor if counter is zero but motor still active
*/
void process_stepper_motors(){
int i, j;
const uint32_t ports[] = {MOTOR_TIM1_PORT, MOTOR_TIM2_PORT};
const uint32_t pins[] = {MOTOR_TIM1_PIN, MOTOR_TIM2_PIN};
const uint8_t startno[] = {0, 3};
const uint8_t stopno[] = {3, 5};
//static uint8_t showcurpos[5] = {0,0,0,0,0};
uint8_t curpos;
const uint32_t Tim[2] = {TIM3, TIM4};
for(j = 0; j < 2; j++){
// new period of motors' timer -- maximum value for all periods in group
uint16_t new_period = 0;
if(timer_flag[j]){
timer_flag[j] = 0;
uint8_t is_active = 0;
for(i = startno[j]; i < stopno[j]; i++)
if(Motor_active[i]) is_active = 1;
if(!is_active) continue; // don't generate clock pulses when there's no moving motors
if(undervoltage_test(MOTORS_VOLTAGE_ALERT)){ // UNDERVOLTAGE! Stop all active motors
for(i = 0; i < 5; i++)
if(Motor_active[i]) stop_motor(i);
return;
}
gpio_toggle(ports[j], pins[j]); // change clock state
if(!gpio_get(ports[j], pins[j])){ // negative pulse - omit this half-step
continue;
}
for(i = startno[j]; i < stopno[j]; i++){ // check motors
if(Motor_active[i] == 0) continue; // inactive motor
curpos = check_ep(i);
if(Motor_steps[i] == 0){ // end of moving
stop_motor(i); // even if this is a turret with move2pos[i]!=0 we should stop
//(what if there's some slipping or so on?)
}else{ // we should move further
if(waits[i]){ // waiting for position stabilisation
uint8_t got_new_position = 0;
waits[i]--;
if(waits[i]) continue; // there's more half-steps to skip
// tell user current position if we was stopped at fixed pos
if(lastpos[i] == 0 && curpos != 0){
got_new_position = 1;
MSG("position of motor ", "[ " STR_ENDSW_STATE " ");
print_int(i, lastsendfun);
lastsendfun(' ');
print_int(curpos, lastsendfun);
if(mode == LINE_MODE) P(" ]", lastsendfun);
lastsendfun('\n');
}
lastpos[i] = curpos;
// turn on motor after pause
gpio_set(MOTOR_EN_PORT, MOTOR_EN_PIN(i));
if(j == 1){ // this is a linear stage
if(test_stages_endpos(i, curpos)){ // this is the end of way
stop_motor(i);
}
}else{ // this is a turret
if(move2pos[i]){ // we should move to specific position
if(curpos == move2pos[i]){ // we are on position
stop_motor(i);
}else if(got_new_position){ // add some steps to move to next position
if(++positions_pass[i] > MAX_POSITIONS_PASS){
ERR("Can't reach given position");
stop_motor(i);
}else
Motor_steps[i] += TURRETS_NEXT_POS_STEPS;
}
}
}
}else{
// check for overcurrent: if MOTOR_EN_PIN == 0
if(!gpio_get(MOTOR_EN_PORT, MOTOR_EN_PIN(i))){
ERR("overcurrent\n");
stop_motor(i);
continue;
}
if(lastpos[i] != curpos){ // transition process
if(lastpos[i] == 0){ // come towards position
if(j == 0){ // this is a turret: make pause & prepare acceleration for start
waits[i] = Turrets_pause;
accel[i] = START_MOTORS_ACCEL_IDX_4;
}else{
waits[i] = 1;
}
// turn off motor while a pause (turret will be locked at fixed position by spring)
// for this short pause we can simply do a pulldown
gpio_clear(MOTOR_EN_PORT, MOTOR_EN_PIN(i));
continue;
}
lastpos[i] = curpos;
}
Motor_steps[i]--;
// change value of current motor's position
Motor_abs_steps[i] += Motor_step_increment[i];
if(accel[i]){ // we are starting
uint32_t NP = (uint32_t)Motor_period[j] * accel_mults[(accel[i]--)/4];
if(NP > 0xffff) NP = 0xffff;
if(new_period < NP) new_period = (uint16_t)NP;
}
}
}
}
if(new_period){ // we have to change motors' speed when accelerating
timer_set_period(Tim[j], new_period);
}
}
}
}
/**
* Move motor Motor_number to User_value steps
* return 0 if motor is still moving
*/
uint8_t move_motor(uint8_t num, int32_t steps){
uint8_t curpos, negative_dir = 0, N_active_in_group = 0;
if(steps == 0) return 0;
// check whether motor is moving
/* if(Motor_active[num]){
ERR("moving\n");
return 0;
}*/
// don't move motors if there's no power enough
if(undervoltage_test(MOTORS_VOLTAGE_THRES)) return 0;
if(num < 3){
for(curpos = 0; curpos < 4; curpos++)
if(Motor_active[curpos]) N_active_in_group++;
}else{
if(Motor_active[3] || Motor_active[4]) N_active_in_group = 1;
}
if(N_active_in_group){ // we can't move: there's any active motor in group
ERR("moving\n");
return 0;
}
#ifdef EBUG
if(mode == BYTE_MODE){
P("move ", lastsendfun);
lastsendfun('0' + num);
P(" to ", lastsendfun);
print_int(steps, lastsendfun);
lastsendfun('\n');
}
#endif
if(steps < 0){
negative_dir = 1;
Motor_step_increment[num] = -1;
steps = -steps;
}else
Motor_step_increment[num] = 1;
curpos = check_ep(num);
lastpos[num] = curpos;
if(negative_dir){
gpio_set(MOTOR_DIR_PORT, MOTOR_DIR_PIN(num)); // set DIR bit to rotate ccw
}else{
gpio_clear(MOTOR_DIR_PORT, MOTOR_DIR_PIN(num)); // reset DIR bit
}
if(test_stages_endpos(num, curpos)){ // error: we can't move
stop_motor(num); // say about it
return 0;
}
// set all flags and variables
Motor_steps[num] = steps; // we run in full-step mode!
waits[num] = 0;
accel[num] = START_MOTORS_ACCEL_IDX_4;
Motor_active[num] = 1;
if(num < 3) // this is turret -> reset counter of passed positions
positions_pass[num] = 0;
// pullup input when active
gpio_set_mode(MOTOR_EN_PORT, GPIO_MODE_INPUT,
GPIO_CNF_INPUT_PULL_UPDOWN, MOTOR_EN_PIN(num));
gpio_set(MOTOR_EN_PORT, MOTOR_EN_PIN(num));
/*
P("set: ", lastsendfun);
print_int(GPIO_ODR(MOTOR_EN_PORT) & MOTOR_EN_MASK, lastsendfun);
P(", get: ", lastsendfun);
print_int(GPIO_IDR(MOTOR_EN_PORT) & MOTOR_EN_MASK, lastsendfun);
lastsendfun('\n');
*/
return 1;
}
int main() {
unsigned long T = 0L;
int Ival;
U8 rb, Num;
CFG_GCR |= 1; // disable SWIM
// Configure clocking
CLK_CKDIVR = 0; // F_HSI = 16MHz, f_CPU = 16MHz
// Timer 4 (8 bit) used as system tick timer
// prescaler == 128 (2^7), Tfreq = 125kHz
// period = 1ms, so ARR = 125
TIM4_PSCR = 7;
TIM4_ARR = 125;
// interrupts: update
TIM4_IER = TIM_IER_UIE;
// auto-reload + interrupt on overflow + enable
TIM4_CR1 = TIM_CR1_APRE | TIM_CR1_URS | TIM_CR1_CEN;
// Configure pins
// PC2 - PP output (on-board LED)
PORT(LED_PORT, DDR) |= LED_PIN;
PORT(LED_PORT, CR1) |= LED_PIN;
// PD5 - UART2_TX -- pseudo open-drain output; don't forget an pullup resistor!
PORT(UART_PORT, DDR) |= UART_TX_PIN;
PORT(UART_PORT, ODR) |= UART_TX_PIN; // torn off N push-down ???
//PORT(UART_PORT, CR1) |= UART_TX_PIN;
// Configure UART
// 9 bit, no parity, 1 stop (UART_CR3 = 0 - reset value)
// 57600 on 16MHz: BRR1=0x11, BRR2=0x06
UART2_BRR1 = 0x11; UART2_BRR2 = 0x06;
UART2_CR1 = UART_CR1_M; // M = 1 -- 9bits
UART2_CR2 = UART_CR2_REN | UART_CR2_RIEN; // Allow RX, generate ints on rx
setup_stepper_pins();
// enable all interrupts
enableInterrupts();
// Loop
do{
if((Global_time - T > paused_val) || (T > Global_time)){
T = Global_time;
PORT(LED_PORT, ODR) ^= LED_PIN; // blink on-board LED
}
if(UART_read_byte(&rb)){ // buffer isn't empty
switch(rb){
case 'h': // help
case 'H':
uart_write("\nPROTO:\n"
"+/-\tLED period\n"
"Ex/ex\tset/get end-switches stored\n"
"p\tget HW end-switches\n"
"Mx\tstop on end-switch\n"
"Sx/sx\tset/get Mspeed\n"
"mx\tget steps\n"
"Px\tpause/resume\n"
"Xx\tstop\n"
"0..2N\tmove xth motor for N steps\n"
"=\tinfinity moving (after 0..2)"
"U/u\tset/get U-stepping\n"
"I\tget serial ID\n"
"N\tchange HW number\n"
"n\tshow HW number\n"
);
break;
case 'I': // get serial id
show_uid();
break;
case '+':
paused_val += 100;
if(paused_val > 10000)
paused_val = 500; // but not more than 10s
break;
case '-':
paused_val -= 100;
if(paused_val < 100) // but not less than 0.1s
paused_val = 500;
break;
case 'E': // set end-switches value
if(get_motor_number(&Num)){
if(readInt(&Ival) && (Ival == (Ival & 0x1f))){
if(Num)
EPs[Num] = Ival & 0x0f; // 4 bits in motors 1&2
else
EPs[0] = Ival; // all 5 bits in motor 0
}else
error_msg("bad EP");
}
break;
case 'e': // get stored end-switches value
if(get_motor_number(&Num)){
printUint(&EPs[Num], 1);
}
break;
case 'p': // get hardware end-switches value
if(get_motor_number(&Num)){
Num = get_ep_value(Num);
printUint(&Num, 1);
}
break;
case 'S': // set stepper speed
if(get_motor_number(&Num)){
if(readInt(&Ival) && Ival > MIN_STEP_LENGTH)
set_stepper_speed(Num, Ival);
else
error_msg("bad speed");
}
break;
case 's': // get stepper speed
if(get_motor_number(&Num))
printUint((U8*)&Stepper_speed[Num], 2);
break;
case 'M': // move till EP, you can call it before starting motor
if(get_motor_number(&Num))
Stop_on_EP[Num] = 1;
break;
case 'm': // how much steps there is to the end of moving
if(get_motor_number(&Num))
printUint((U8*)&Nsteps[Num], 2);
break;
case 'X': // stop
if(get_motor_number(&Num))
stop_motor(Num);
break;
case 'P': // pause/resume
if(get_motor_number(&Num))
pause_resume(Num);
break;
case 'N':
if(readInt(&Ival) && Ival > 0 && Ival < 256)
if(!change_progmem_value(&UART_devNUM, (unsigned int) Ival))
error_msg("can't change val");
break;
case 'n': // show HW num
printUint(&UART_devNUM, 1);
break;
case 'u': // show UStepping
printUint(&USteps, 1);
break;
case 'U': // set UStepping
if(readInt(&Ival) && Ival > 0 && Ival < 256)
USteps = Ival;
break;
case '=': // infinity moving: just don't decrement steps
StepperInfty = 1;
break;
default:
if(rb >= '0' && rb <= '2'){ // run motor
Num = rb - '0';
if(readInt(&Ival) && Ival)
move_motor(Num, Ival);
else{
error_msg("bad Nsteps");
}
}
}
}
}while(1);
}
void text_interface(void)
{
char cmd[MAX_CMD_LEN];
uint8_t speed, current_sense;
uint16_t ticks;
uint16_t t;
uint8_t i, cs;
for(i = 0; i < 5; i++)
{
set_led_rgb(0, 0, 255);
_delay_ms(150);
set_led_rgb(0, 0, 0);
_delay_ms(150);
}
set_led_pattern(LED_PATTERN_IDLE);
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
setup();
stop_motor();
serial_init();
cs = 0;
sei();
_delay_ms(10);
dprintf("\nParty Robotics Dispenser at your service!\n\n");
for(;;)
{
cli();
cs = g_current_sense_detected;
sei();
if (!receive_cmd(cmd))
break;
if (sscanf(cmd, "speed %hhu %hhu", &speed, ¤t_sense) == 2)
{
if (!cs)
set_motor_speed(speed, current_sense);
if (current_sense == 0)
flush_saved_tick_count(0);
continue;
}
if (sscanf(cmd, "tickdisp %hu %hhu", (short unsigned int *)&ticks, &speed) == 2)
{
if (!cs)
{
dispense_ticks(ticks, speed);
flush_saved_tick_count(0);
}
continue;
}
if (sscanf(cmd, "timedisp %hu", (short unsigned int *)&t) == 1)
{
if (!cs)
{
run_motor_timed(t);
flush_saved_tick_count(0);
}
continue;
}
if (strncmp(cmd, "forward", 7) == 0)
{
set_motor_direction(MOTOR_DIRECTION_FORWARD);
continue;
}
if (strncmp(cmd, "backward", 8) == 0)
{
set_motor_direction(MOTOR_DIRECTION_BACKWARD);
continue;
}
if (strncmp(cmd, "led_idle", 8) == 0)
{
set_led_pattern(LED_PATTERN_IDLE);
continue;
}
if (strncmp(cmd, "led_dispense", 12) == 0)
{
set_led_pattern(LED_PATTERN_DISPENSE);
continue;
}
if (strncmp(cmd, "led_done", 8) == 0)
{
set_led_pattern(LED_PATTERN_DRINK_DONE);
continue;
}
if (strncmp(cmd, "led_clean", 8) == 0)
{
set_led_pattern(LED_PATTERN_CLEAN);
continue;
}
if (strncmp(cmd, "help", 4) == 0)
{
dprintf("You can use these commands:\n");
dprintf(" speed <speed> <cs>\n");
dprintf(" tickdisp <ticks> <speed>\n");
dprintf(" timedisp <ms>\n");
dprintf(" forward\n");
dprintf(" backward\n");
dprintf(" reset\n");
dprintf(" led_idle\n");
dprintf(" led_dispense\n");
dprintf(" led_done\n");
dprintf(" led_clean\n\n");
dprintf("speed is from 0 - 255. cs = current sense and is 0 or 1.\n");
dprintf("ticks == number of quarter turns. ms == milliseconds\n");
continue;
}
if (strncmp(cmd, "reset", 5) == 0)
break;
dprintf("Unknown command. Use help to get, eh help. Duh.\n");
}
}
}
void stop_r3d2(void)
{
stop_motor();
disable_sensor();
}
