int8_t set_motor_levels(int16_t motor_speed_level,
uint16_t motor_direction_level)
{
motor_levels.speed_channel_level = motor_speed_level;
motor_levels.direction_channel_level = motor_direction_level;
motor_timestamp = get_current_time();
if (motor_speed_level == MOTOR_LEVEL_BRAKE)
{
start_braking();
}
else if (motor_speed_level > 0) // if want to go forward
{
// if already going forward or stopped
if ((motor_direction == MOTOR_DIRECTION_FORWARD) || (motor_direction
== MOTOR_DIRECTION_STOPPED))
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
else // if trying to change directions
{
start_brake_coasting();
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_speed_level < 0) // if want to go backward
{
if ((motor_direction == MOTOR_DIRECTION_REVERSE) || (motor_direction
== MOTOR_DIRECTION_STOPPED))
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
else // if trying to changed directions
{
start_brake_coasting();
post_error(ERR_MOTOR_FAULT);
}
}
else // if stopping, but not changing directions, then just coast
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_COASTING;
motor_coast_timestamp = get_current_time();
}
return ERR_NONE;
}
void motor_init(void)
{
motor_state = MOTOR_STATE_IDLE;
set_sensor_waiting_for_measurement(true);
set_motor_direction(MOTOR_DIRECTION_UNKNOWN);
if (get_nvm_error_flag() == ERR_NONE)
{
direction_null_value = nvm_data.direction_null_value;
motor_timeout = nvm_data.motor_timeout;
}
else
{
direction_null_value = DEFAULT_DIRECTION_NULL_VALUE;
motor_timeout = DEFAULT_MOTOR_TIMEOUT;
}
set_direction_motor_pwm_level(direction_null_value);
pwm_init();
}
// initialise sensor (returns true if sensor is succesfully initialised)
bool AP_Proximity_LightWareSF40C::initialise()
{
// set motor direction once per second
if (_motor_direction > 1) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_motor_direction();
}
}
// set forward direction once per second
if (_forward_direction != frontend.get_yaw_correction(state.instance)) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_forward_direction();
}
}
// request motors turn on once per second
if (_motor_speed == 0) {
if ((_last_request_ms == 0) || AP_HAL::millis() - _last_request_ms > 1000) {
set_motor_speed(true);
}
return false;
}
return true;
}
void* goto_floor_with_network(int targetFloor){
// Baserer meg her på at heis befinner seg i etasje
//Try to reconnect
//If orders in orderlist
int currentFloor;
int directionIndex;
int flag = 0;
// Funksjoner som sjekker bestillinger kan implementeres som tråder og fjernes fra tilstandsmaskiner
printf("Searching for orders\n");
//While løkke kjører så lenge bestillingslister har elementer i seg
printf("Element in ButtonVector, internalButtonVector: %i externalButtonVector: %i\n", !check_if_internalButtonVector_is_empty(), !check_if_externalButtonVector_is_empty() );
print_button_vectors();
currentFloor = elev_get_floor_sensor_signal();
lastFloor = currentFloor;
printf("TargetFloor etter end_order_in_direction %i \n", targetFloor);
direction = get_direction(currentFloor, targetFloor);
directionIndex = direction_to_index(direction); // Til bruk når vi aksesser externalButtonVector
printf("Direction er %i directionIndex er %i\n", direction, directionIndex );
set_motor_direction(lastFloor, targetFloor);
time_t start, end;
double elapsed = 0;
time(&start);
while(currentFloor != targetFloor){ //Heis beveger seg mot targetFloor i gitt retning
if (elapsed > 5 ){
connectStatus = 0;
printf("Motor Disconnected\n");
while(!((currentFloor != -1) && (currentFloor != lastFloor) )){
elapsed = 0;
time(&start);
currentFloor = elev_get_floor_sensor_signal();
}
connectStatus = 1;
printf("Motor connected\n");
}
else{
connectStatus = 1;
time(&end);
}
elapsed = difftime(end, start);
if((currentFloor != -1) && (currentFloor != lastFloor) ){ //Elevator have reached new floor
time(&start);
lastFloor = currentFloor;
elev_set_floor_indicator(lastFloor);
if (internalButtonVector[currentFloor]){
pthread_mutex_lock(&elevLock);
internalButtonVector[currentFloor] = 0;
pthread_mutex_unlock(&elevLock);
elev_set_motor_direction(DIRN_STOP);
printf("Open door 1\n");
elev_set_button_lamp(BUTTON_COMMAND, currentFloor, 0);
open_door();
flag = 1;
}
if(externalButtonVector[currentFloor][directionIndex] ){
pthread_mutex_lock(&elevLock);
externalButtonVector[currentFloor][directionIndex] = 0;
send_order_handeled_confirmation(currentFloor, directionIndex);
pthread_mutex_unlock(&elevLock);
if (flag != 1){
elev_set_motor_direction(DIRN_STOP);
printf("Open door 1\n");
elev_set_button_lamp(BUTTON_COMMAND, currentFloor, 0);
open_door();
flag = 0;
}
}
}
set_motor_direction(lastFloor, targetFloor);
pthread_mutex_lock(&elevLock);
targetFloor = end_order_in_direction(direction, targetFloor); // Oppdaterer targetfloor under reise
pthread_mutex_unlock(&elevLock);
// printf("LastFloor er %i\n", lastFloor);
currentFloor = elev_get_floor_sensor_signal();
// sleep(0.2); //Forsinke prints
}
//Exit while -> framme ved targetFloor
if ( (currentFloor == targetFloor) ){
lastFloor = currentFloor;
elev_set_floor_indicator(lastFloor);
elev_set_motor_direction(DIRN_STOP);
printf("Open door 2, framme ved targetFloor %i\n", targetFloor);
open_door();
elev_set_button_lamp(BUTTON_COMMAND, targetFloor, 0);
pthread_mutex_lock(&elevLock);
internalButtonVector[targetFloor] = 0;
externalButtonVector[targetFloor][0] = 0; // I denne bestillingsslettingen kan det fort ligge bug
externalButtonVector[targetFloor][1] = 0;
pthread_mutex_unlock(&elevLock);
send_order_handeled_confirmation(targetFloor, 0);
send_order_handeled_confirmation(targetFloor, 1);
direction = 0;
}
// sleep(0.2);
return NULL;
}
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);
}
// Ensure we're running the right software for this board
check_software_revision();
// get the current liquid level
update_liquid_level();
for(;;)
{
cli();
g_reset = 0;
g_current_sense_detected = 0;
g_motor_direction = MOTOR_DIRECTION_FORWARD;
setup();
serial_init();
stop_motor();
pulse_motor_driver_retry();
set_led_rgb(0, 0, 255);
sei();
id = address_exchange();
for(; !check_reset();)
{
rec = receive_packet(id, &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_GET_VERSION:
send_packet8(PACKET_GET_VERSION, SOFTWARE_VERSION);
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_SET_MOTOR_DIRECTION:
if (!cs)
set_motor_direction(p.p.uint8[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((uint16_t)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:
// Only for v2 pumps
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;
}
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 motor_run_handler(uint32_t current_time)
{
if (get_nvm_error_flag() != ERR_NONE)
return;
switch (motor_state)
{
case MOTOR_STATE_IDLE:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
set_sensor_waiting_for_measurement(true);
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
break;
case MOTOR_STATE_BRAKING:
if (check_for_timeout(current_time, motor_brake_timestamp,
MOTOR_BRAKE_TIME))
{
start_brake_coasting();
}
break;
case MOTOR_STATE_BRAKE_COAST:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
debug_P(PSTR("Stop brake coasting @ %lu\n"), current_time);
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_BRAKE_READ_BACK_EMF;
motor_read_back_emf_count = 0;
}
break;
case MOTOR_STATE_BRAKE_READ_BACK_EMF:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
// if we have receive a brake command we just keep braking
if (motor_levels.speed_channel_level == MOTOR_LEVEL_BRAKE)
{
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_levels.speed_channel_level > 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_FORWARD))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
}
else if (motor_levels.speed_channel_level < 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_REVERSE))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
}
else if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
motor_state = MOTOR_STATE_IDLE;
set_motor_direction(MOTOR_DIRECTION_STOPPED);
}
// motor fault is done
if (motor_state != MOTOR_STATE_BRAKE_READ_BACK_EMF)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else if (++motor_read_back_emf_count == BACK_EMF_READ_COUNT)
{
start_braking();
}
else
{
set_sensor_waiting_for_measurement(true);
}
}
break;
case MOTOR_STATE_COASTING:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_IDLE;
}
break;
case MOTOR_STATE_ACTIVE:
if (check_for_timeout(current_time, motor_timestamp, motor_timeout))
{
motor_levels.speed_channel_level = 0;
start_braking();
debug_P(PSTR("MOTOR Timeout @ %lu\n"), get_current_time());
post_error(ERR_MOTOR_TIMEOUT);
}
else
{
set_speed_motor_pwm_level(motor_levels.speed_channel_level);
set_direction_motor_pwm_level(motor_levels.direction_channel_level);
}
break;
}
}
