/* Calculate backemf and save in g_pos
*/
void calculate_backemf (uint8_t motor)
{
int16_t backemf=0;
// Read backemf
adc_init(3);
backemf = read_adc(0x10);
if (backemf & 0x200)
{
backemf = (~(backemf) & (0x03FF))+1;
// backemf -= 512;
}
if (backemf < MOTOR_OFFSET)
backemf = 0;
g_pos[motor] += backemf;
if (g_mode[motor] != MOTOR_STOP)
debug_value(backemf,10);
//Compare voltage
if (((g_angle[motor] > 1)) && ( g_mode[motor] != MOTOR_STOP))
{
if ( g_pos[motor] >= (g_angle[motor])) // Check for position
{
motor_control(1, MOTOR_BRAKE);
delay(3);
g_mode[motor] = MOTOR_STOP;
uart_puts ("Voltage SUM");
debug_value(g_pos[motor],10);
g_pos[motor] =0;
}
}
}
void File::plan() {
debug_value("Vykdomas failo planuotojas: ", this->id);
debug_value("Failo vardas: ", this->file_name);
if (!this->is_free()) {
debug_string("\tFailas užimtas.\n");
return;
}
while (!this->process_queue.is_empty()) {
ProcessContainer container = process_queue.pop_front();
if (container.process_id == 0) {
debug_string("\tProcesas jau nebegyvas.\n");
}
else {
this->set_free(false);
this->reset();
this->file_manager->give_file(
container.process_id, this->id, container.mode);
this->file_manager->activate_process(container.process_id);
debug_value("\tAtiduotas procesui: ", container.process_id);
break;
}
}
}
Value eval(Value expression, Environment *environment) {
switch (expression.type) {
/* Self evaluating: */
case ERROR:
case NIL:
case INTEGER:
case FLOAT:
case STRING:
case VECTOR:
case HASH:
case LAMBDA: // Evaluation is not the same as calling
default:
return expression;
case SYMBOL: {
Value result;
Bool found = environment_lookup_variable(expression, &result, environment);
if (found) {
return result;
} else {
/* TODO: log error */
/* TODO: "Did you mean?" */
debug_value(expression);
log_error("Variable XXX not found");
return VALUE_ERROR;
}
}
case CONS:
return eval_list(expression, environment);
}
}
Value eval_list(Value expression, Environment *environment) {
w_assert(expression.type == CONS);
Bool lambda_call = false;
Value function_symbol = NEXT(expression);
Value lambda;
Function *function;
if (function_symbol.type == CONS) {
if (CAR(function_symbol).type == SYMBOL &&
CAR(function_symbol).val.symbol_val == symbols_lambda.val.symbol_val) {
lambda = eval(function_symbol, environment);
lambda_call = true;
} else {
return VALUE_ERROR;
}
} else if (function_symbol.type == LAMBDA) {
lambda = function_symbol;
lambda_call = true;
} else if (function_symbol.type == SYMBOL) {
Value function_value;
Bool found = hash_get(environment -> functions, function_symbol, &function_value);
if (!found) {
/* TODO: log error */
/* TODO: "Did you mean?" */
debug_value(function_symbol);
log_error("Function XXX not found");
return VALUE_ERROR;
}
w_assert(function_value.type == FUNCTION);
function = function_value.val.function_val;
} else {
return VALUE_ERROR;
}
Value args;
if (lambda_call || function -> eval) {
args = VALUE_NIL;
while (expression.type == CONS) {
Value arg = NEXT(expression);
args = CONS(eval(arg, environment), args);
}
args = list_reverse(args);
w_assert(expression.type == NIL);
/* TODO: benchmark, which approach is better, the above or below? */
/* if (expression.type == CONS) { */
/* args = CONS1(VALUE_NIL); */
/* } else { */
/* args = expression; */
/* } */
/* Cons *top = args.val.cons_val; */
/* while (true) { */
/* Value arg = NEXT(expression); */
/* top -> car = eval(arg, environment); */
/* if (expression.type == CONS) { */
/* top -> cdr = CONS1(VALUE_NIL); */
/* top = top -> cdr.val.cons_val; */
/* } else { */
/* top -> cdr = expression; */
/* break; */
/* } */
/* } */
} else {
/* To ensure we avoid mutation in altering code the list is copied
If we guaranteed that no function with eval = false modifies the list we could give it directly
This would be an obvious performance optimization. But needs tests.
We can only guarantee this for c_code, not for userdefined macros.
TODO: Do this */
args = list_copy(expression);
}
Value result;
if (lambda_call) {
result = eval_lambda(lambda, args, environment);
} else {
result = eval_apply(function_symbol, function, args, environment);
}
list_destroy(args);
return result;
}
int16_t cmd_decode (uint8_t *buffer_ptr)
{
uint8_t cmd_no =0;
uint8_t *cmd_argument;
uint8_t i,j ;
// Should repeat until end of buffer
// Loop until end of text
cmd_argument = buffer_ptr;
while ( cmd_no != CMD_LINE_END) // Until end of line which mean g_motor will change
{
cmd_no = text_decode (buffer_ptr,cmd_argument);
#ifdef DEBUG
uart_puts("Command :");
uart_putc(0x30+cmd_no);
uart_putc('*');
uart_puts(cmd_argument);
#endif
switch (cmd_no)
{
case MOTOR_CMD:
g_motor = (uint8_t)atoi(cmd_argument);
if (g_motor > 0)
g_motor--; // > 0 Start from 0 need -1
else
g_motor = 0;
break;
case SPEED_CMD:
i = (uint8_t)atoi(cmd_argument);
if ( i >= MOTOR_MAX_SPEED)
i = MOTOR_MAX_SPEED; // Start from 0
if (i) // i > 0;
i--; //Start from 0;
g_speed[g_motor] = g_speed_table[i];
if (g_speed[g_motor] == 0)
g_mode[g_motor] = MOTOR_STOP;
break;
case ANGLE_CMD: // Can be -
g_angle[g_motor] = atoi(cmd_argument);
if (g_angle[g_motor] < 0)
{
g_mode[g_motor] = MOTOR_BACKWARD;
// Set g_angle to positive value by invert all bit and +1
g_angle[g_motor] = (~(g_angle[g_motor])+1);
if (g_angle[g_motor] > 1)
{
g_angle[g_motor] *= MOTOR_ANGLE_COEF;
g_status[g_motor] = 1;
g_speed[g_motor] = g_speed_table[MOTOR_ANGLE_SPEED] ;
}
}
else if (g_angle[g_motor] == 0)
{
g_mode[g_motor] = MOTOR_STOP;
}
else
{
if ((g_angle[g_motor] > 0)) // +
{
g_mode[g_motor] = MOTOR_FORWARD;
}
if (g_angle[g_motor] > 1)
{
g_angle[g_motor] *= MOTOR_ANGLE_COEF;
g_status[g_motor] = 1;
g_speed[g_motor] = g_speed_table[MOTOR_ANGLE_SPEED] ;
}
}
g_pos[g_motor] = 0;
break;
case PROGRAM_CMD:
g_program_flag = (uint8_t)atoi(cmd_argument);
if (g_program_flag) // Start program save in buffer
{
uart_puts ("Program Start to save : ");
g_program_index = 0;
}
else
{
uart_puts ("Program End : ");
}
break;
case SAVE_CMD:
j = (uint8_t)atoi(cmd_argument);
if ( j > 0) // Save commmand to eeprom
{
g_program_buff[g_program_index] = 0x00 ; // Put null at the end
g_program_index++;
for ( i =0; i < g_program_index; i++)
{
while (!eeprom_is_ready());
eeprom_write_byte(&g_program_eeprom[i],g_program_buff[i]);
}
debug_value( g_program_index,10);
uart_puts ("Save complete ");
g_program_index = 0;
}
break;
case RUN_CMD:
program_run();
break;
default:
uart_putc(0x0D);
uart_putc(0x0A);
break;
}
}
return 0;
}
void main()
{
uint8_t sw1,sw2;
uint16_t memaddr = 0; // Start memory Address
uint8_t dat,c,i;
/*
* Initialize UART library, pass baudrate and AVR cpu clock
* with the macro
* UART_BAUD_SELECT() (normal speed mode )
* or
* UART_BAUD_SELECT_DOUBLE_SPEED() ( double speed mode)
*/
DDRC = 0xFC;
uart_init();
d7segment_init();
i2ceeprom_init();
// i2c_set_localdeviceaddr(I2C_EEPROM_MASTER_ADDR,FALSE);
// i2c_set_localdeviceaddr(I2C_EEPROM_SLAVE_ADDR,FALSE);
sei();
// To Wrie and read the same device. Need to delay. Maybe the wait state is not correct....
// dat = 0x44;
// memaddr = 0;
// i2ceeprom_write_byte(I2C_EEPROM_SLAVE_ADDR, memaddr,dat);
// debug_value (dat,16);
// _delay_ms(20);
// dat = i2ceeprom_read_byte(I2C_EEPROM_SLAVE_ADDR,memaddr);
// debug_value (dat,16);
while (1)
{
sw1 = _7SEGMENT_SW1_IN_PORT & _7SEGMENT_SW1;
sw2 = _7SEGMENT_SW2_IN_PORT & _7SEGMENT_SW2;
// Delay
if (!sw1) // Start copy eeprom
{
i = 0;
for (memaddr = 0 ; memaddr < E_24LC32_MEM_ADDR ;memaddr++)
{
dat = i2ceeprom_read_byte(I2C_EEPROM_MASTER_ADDR,memaddr);
debug_value (dat,16);
i2ceeprom_write_byte(I2C_EEPROM_SLAVE_ADDR, memaddr,dat);
// _delay_ms( EEPROM_DELAY);
if (i < 100)
{
c =0;
}
if (i > 100)
{
c ='-';
}
if (i > 200)
i = 0;
d7segment_display(c,1);
_delay_ms(2);
d7segment_display(c,2);
_delay_ms(2);
i++;
}
//
}
// Finish copy
d7segment_display(0,2);
_delay_ms( 10);
d7segment_display(0,1);
_delay_ms( 10);
}
}
/* Calculate backemf and save in g_pos
*/
void calculate_backemf (uint8_t motor)
{
int16_t backemf=0;
// Read backemf
static uint8_t i = 0;
adc_init(1);
backemf = read_adc(g_adc_channel[motor]);
// Cut offset
if ((backemf > 0) && (backemf < MOTOR_OFFSET))
backemf =0;
// Negative convert to positive with 2 complement
if (g_mode[motor] == MOTOR_STOP)
{
if (backemf & 0x200)
{
backemf = (~(backemf) & (0x03FF))+1;
}
}
if (g_mode[motor] == MOTOR_FORWARD) // Motor stop backemf - Forward backemf +
{
if (backemf & 0x200)
{
backemf = (~(backemf) & (0x03FF))+1;
}
else
backemf = 0;
}
if (g_mode[motor] == MOTOR_BACKWARD) // Motor stop backemf + backward backemf -
{
if (backemf & 0x200)
{
backemf =0;
}
}
g_pos[motor] += backemf;
if (g_mode[motor] != MOTOR_STOP)
{
i++;
if (i == 100 )
{
uart_putc(motor+0x30);
uart_putc('-');
debug_value(backemf,10);
i = 0;
}
}
//Compare voltage
if (((g_angle[motor] > 1)) && ( g_mode[motor] != MOTOR_STOP))
{
if ( g_pos[motor] >= (g_angle[motor])) // Check for position
{
// delay(3);
g_mode[motor] = MOTOR_STOP;
uart_puts ("Voltage SUM");
debug_lvalue(g_pos[motor],10);
g_pos[motor] =0;
}
}
}