void assertADXLConversionTrigger(void) {
set_trigger();
/*rprintf("PINSEL0:%x\n\r", PINSEL0);
rprintf("IOPIN0:%x\n\r", IOPIN0);
rprintf("IOSET0:%x\n\r", IOSET0);
rprintf("IODIR0:%x\n\r", IODIR0);
while(1) {}*/
//rprintf("trigger set\n\r");
}
MenuEntryShortcutActionData::MenuEntryShortcutActionData(
ActionDataGroup* parent,
const QString& name,
const QString& comment,
const KShortcut& shortcut,
const QString& menuentry)
: base(parent, name, comment)
{
set_action( new MenuEntryAction( this, menuentry ));
set_trigger( new ShortcutTrigger( this, shortcut ));
}
void set_trg_level(int l)
{
/* variables */
/* none */
/* set the trigger level */
level = l;
/* set the trigger level in hardware too */
set_trigger(level, slope);
/* all done initializing the trigger level - return */
return;
}
void set_trg_slope(enum slope_type s)
{
/* variables */
/* none */
/* set the slope type */
slope = s;
/* also tell the hardware what the slope is */
set_trigger(level, slope);
/* all done setting the trigger slope - return */
return;
}
void trg_level_down()
{
/* variables */
/* none */
/* decrease the trigger level, if not already the minimum */
if (level > MIN_TRG_LEVEL_SET)
level--;
/* set the trigger level for the hardware */
set_trigger(level, slope);
/* all done with lowering the trigger level - return */
return;
}
void trg_level_up()
{
/* variables */
/* none */
/* increase the trigger level, if not already the maximum */
if (level < MAX_TRG_LEVEL_SET)
level++;
/* tell the hardware the new trigger level */
set_trigger(level, slope);
/* all done raising the trigger level - return */
return;
}
void trg_slope_toggle()
{
/* variables */
/* none */
/* toggle the trigger slope */
if (slope == SLOPE_POSITIVE)
slope = SLOPE_NEGATIVE;
else
slope = SLOPE_POSITIVE;
/* set the new trigger slope */
set_trigger(level, slope);
/* all done with the trigger slope - return */
return;
}
int main()
{
init();
while(1)
{
switch(state)
{
case STATE_GET_COMMAND:
UARTprintf(">");
get_command();
state = decode_command();
break;
case STATE_PRINT_INFO:
state = print_info();
break;
case STATE_SET_MODE:
state = set_mode();
break;
case STATE_SET_INTERVAL:
state = set_interval();
break;
case STATE_SET_TRIGGER:
state = set_trigger();
break;
case STATE_RUN:
state = run();
break;
case STATE_STOP:
state = stop();
break;
case STATE_GET_BUFFER:
state = get_buffer();
break;
case STATE_GET_PARTIAL_BUFFER:
state = get_partial_buffer();
break;
case STATE_PRINT_STATE:
state = print_state();
break;
case STATE_LED:
led_mask = led_mask ? 0 : 0xffffffff;
state = STATE_GET_COMMAND;
break;
case STATE_RESET:
reset();
state = STATE_GET_COMMAND;
break;
case STATE_UNKNOWN_COMMAND:
UARTprintf("UNKNOWN COMMAND\r\n");
state = STATE_GET_COMMAND;
break;
}
}
return 0;
}
/**
* Drive instruction handler function.
*
* Very long but versitile function used to drive around the world
* (maybe not in 80 days).
* @param[in,out] order The order which specifies what exactly
* should be done.
* @todo The function is partially really ugly. Refactor it.
*/
void drive_instruction(order_t *order) {
// Extract the type of triggers to be used
uint8_t trigger_type_left = order->data[0] & 0x30;
uint8_t trigger_type_right = order->data[0] & 0xc0;
// Import the option for ABS
extern uint8_t ACTIVE_BRAKE_WHEN_TRIGGER_REACHED;
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_type_left = "), trigger_type_left);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_type_right = "), trigger_type_right);
}
// This if shouldn't be taken, just to make sure
if (order->status & ORDER_STATUS_DONE) {
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_DONE\n"));
}
return;
}
// Instruciton is already running
if (order->status & ORDER_STATUS_STARTED) {
// Local variable to save the checkTrigger return value for debug output
uint16_t result = 0;
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_STARTED\n"));
}
// Check Left trigger
result = check_trigger(trigger_type_left, WHEEL_LEFT);
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : checkTrigger(LEFT) = "), result);
}
// Left Trigger reached in this Iteration?
if (!(order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) &&
result == 0) {
// Set the Trigger reached flag
order->status |= ORDER_STATUS_TRIGGER_LEFT_REACHED;
// set the ABS ref position
drive_brake_active_set(WHEEL_LEFT);
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_TRIGGER_LEFT_REACHED\n"));
}
}
// Right trigger reached
result = check_trigger(trigger_type_right, WHEEL_RIGHT);
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : checkTrigger(RIGHT) = "), result);
}
// Right Trigger reached in this Iteration?
if (!(order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED) &&
result == 0 ) {
// Set the Trigger reached flag
order->status |= ORDER_STATUS_TRIGGER_RIGHT_REACHED;
// set the ABS ref position
drive_brake_active_set(WHEEL_RIGHT);
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : status = ORDER_STATUS_TRIGGER_RIGHT_REACHED\n"));
}
}
// If in PID with D mode one reached trigger equals all trigger reached
if ((order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED ||
order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) &&
trigger_type_left == 0x30) {
order->status |= ORDER_STATUS_TRIGGER_LEFT_REACHED + ORDER_STATUS_TRIGGER_RIGHT_REACHED;
}
// both triggers reached, ergo order is done
if (order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED &&
order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) {
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : All trigger reached\n"));
}
order->status |= ORDER_STATUS_DONE;
// stop the wheels
drive_use_pid(WHEEL_BOTH, 0);
wheel_clear_difference();
}
// When it is wanted, we will use active breaking on the wheel where the trigger has been reached
if (ACTIVE_BRAKE_WHEN_TRIGGER_REACHED) {
if (order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED) {
drive_brake_active(WHEEL_LEFT);
} else if (order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED) {
drive_brake_active(WHEEL_RIGHT);
}
}
// Use error-correction while driving every 100 ms
if (local_time_flags & TIMER_100MS) {
if (trigger_type_left == 0x30) { // Use function for both wheels to use PID mode with D
drive_use_pid(WHEEL_BOTH, order->data[1]);
} else {
// Only drive the wheels if the triggers haven't been reached
if (!(order->status & ORDER_STATUS_TRIGGER_LEFT_REACHED)) {
drive_use_pid(WHEEL_LEFT, order->data[1]);
}
if (!(order->status & ORDER_STATUS_TRIGGER_RIGHT_REACHED)) {
drive_use_pid(WHEEL_RIGHT, order->data[2]);
}
}
}
} else { //Instruction isn't running
// Build the values for the specified triggers, even if there aren't any triggers present
int16_t trigger_value_left = 0;
int16_t trigger_value_right = 0;
if (DEBUG_ENABLE) {
debug_write_string_p(PSTR("order_functions.c : drive_instruction() : Start execution\n"));
debug_write_integer(PSTR("order_functions.c : drive_instruction() : speed_left = "), order->data[1]);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : speed_right = "), order->data[2]);
}
// -128 is not a accepted speed, so we correct it down by 1
if (order->data[1] == 128) {
order->data[1] = -127;
}
if (order->data[2] == 128) {
order->data[2] = -127;
}
trigger_value_left = ((order->data[3] << 8) + order->data[4]);
trigger_value_right = ((order->data[5] << 8) + order->data[6]);
if (trigger_type_right == 0xc0) { // Set new differential correction
trigger_value_left = (order->data[1] << 8) + order->data[2];
wheel_write_difference(trigger_value_left);
// We are done with this order
order->status |= ORDER_STATUS_DONE;
return;
}
if (trigger_type_left == 0x30) { // PID with differential correction
trigger_value_left = trigger_value_right = ((order->data[2] << 8) + order->data[3]);
}
// Set the triggers for each wheel
if (DEBUG_ENABLE) {
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_value_left = "), trigger_value_left);
debug_write_integer(PSTR("order_functions.c : drive_instruction() : trigger_value_right = "), trigger_value_right);
}
// Set the Trigger values
set_trigger(trigger_type_left, WHEEL_LEFT, trigger_value_left);
set_trigger(trigger_type_right, WHEEL_RIGHT, trigger_value_right);
// Set Status to started as setup is complete
order->status |= ORDER_STATUS_STARTED;
}
}
UltrasonicSensor::UltrasonicSensor(int echo_pin,int trigger_pin)
{
set_echo(echo_pin);
set_trigger(trigger_pin);
}
