/**
Sets the target current of the PID current controller to @c current as a fixed point value.
The current controller will attempt to achieve this current on the next call to @ref mc_pid_current.
This function is faster than @ref mc_set_target_current, and its use is preferred.
@param current The target current in fixed point.
*/
static void mc_set_target_current_fixed(fixed current){
// fixed curr = mc_data.operation * current;
fixed curr = current;
if (curr < mc_data.target_min){
error_occurred(ERROR_MC_TCURR_OOB);
curr = mc_data.target_min;
}
if (curr > mc_data.target_max){
error_occurred(ERROR_MC_TCURR_OOB);
curr = mc_data.target_max;
}
mc_target_current = curr;
}
/**
Turns on the motor controller once it is safe to turn back on.
Sets the appropriate state and causes an error. Starts feeding the
watchdog timer again. Also turns off the green LED.
@param reason Why this function was called.
@private
*/
static void mc_turnon(MC_SAFETY_CAUSE reason){
switch(reason){ //set appropriate state and cause error
case MC_THERM: mc_therm_off = 0;
error_occurred(ERROR_MC_TEMP_ON);
break;
case MC_MECH: mc_mech_off = 0;
error_occurred(ERROR_MC_MECH_ON);
break;
default: //shouldn't get here
break;
}
mcu_led_green_off();
}
//Sends all of the bytes
void msimu_send_all(unsigned char *bytes, unsigned int length)
{
unsigned int i;
if ((length <= 16)) //length must be smaller than 16-byte FIFO and FIFO must be empty
{
if (U1LSR & (1<<5)){
for (i=0;i<length;i++){
U1THR = *(bytes + i);
}
} else {
error_occurred(ERROR_MSIMU_TRANSMIT_FULL, PRIORITY_MED);
}
} else {
error_occurred(ERROR_MSIMU_TOO_MANY_BYTES, PRIORITY_HIGH);
}
}
/**
Calculates a thermal limiting multiplier for use by @ref mc_pid_current.
Using the @c thermal_current_limit and @c thermal_time_constant passed to @ref mc_init,
this function will estimate the current temperature of the motor and find a multiplier
that when multiplied by the current will limit the temperature. If the motor is too hot,
this function will call @ref mc_turnoff to completely shutdown the motor until it cools down.
@warning This function uses the motor current and Newton's Law of Cooling to estimate the temperature,
it doesn't measure the motor temperature directly, so care should be taken to let the motors cool down when
the robot is turned off and on, as the residual heat of the motors will not be taken into account.
@param current The newest motor current to add to the thermal checking.
*/
static fixed mc_get_thermal_limiter(fixed current){
//Check max current for thermal shutoff
//Must create current_factor, thermal_safe, thermal_limit
static fixed multiplier = FIXED_ONE;
static fixed a = 0; // a is C/R*(temperature - environmental temperature), C is heat capacity of motor, R is electric resitance which causes the heating I^2*R
// DANGEREROUS TO INITIALIZE A to zero, eg after lettting the robo on for a while, then switching off and then turning on again
fixed a_dot_dt; //derivative of 'a' multiplied by time step dt
fixed motor_current = current;
a_dot_dt = fixed_mult((fixed_mult(motor_current, motor_current)-fixed_mult(mc_data.tau_inv, a)), mc_data.dt); // derivative is -(1/tau)*a + I^2 , this is newton's law of cooling
a = a + a_dot_dt; //
if (a < mc_data.thermal_safe){ //below limit, full current
multiplier = FIXED_ONE;
if (mc_therm_off){ //previously turned off by thermal limit, but safe again
mc_turnon(MC_THERM); //turn on controller
}
} else if (a >= mc_data.thermal_safe && a < mc_data.thermal_limit){ //above safety limit, use mult factor
multiplier = FIXED_ONE - (fixed_mult(a - mc_data.thermal_safe, mc_data.thermal_diff)); //mc.thermal_diff = float_to_fixed(1.0/(thermal_limit - thermal_safe))
error_occurred(ERROR_MC_TEMP_SAFE);
} else if (a >= mc_data.thermal_limit){ //above strict limit, shutdown
mc_turnoff(MC_THERM); //turn off controller due to thermal limit
multiplier = 0;
} else {
multiplier = FIXED_ONE; //should never reach here
}
return multiplier;
}
/*
* Called when a sync timestamp packet
* is received from the main brain. Resyncs the
* scheduler if it is asynchronous.
*/
void sched_sync(void){
if (sched_status == SCHED_RUNNING_ASYNC){
error_occurred(ERROR_SCHED_RESYNC, PRIORITY_HIGH);
sched_status = SCHED_RUNNING_SYNC;
}
sched_aux_count = 0;
sched_sync_count = 0;
sched_tick();
}
void init_can(void){
// ************* First, fill regular RX descriptors ***************** //
int i = 0;
//Timestamp
rxlist[i++] = &rx_fd_timestamp;
can_set_rx_descriptor_fi(&rx_fd_timestamp, ID_TIMESTAMP, CHAN_CAN1, can_rx_setter_float_dummy, dn_rx_timestamp);
//Add NULL terminator
rxlist[i++] = (CAN_FRAME_DESC *) 0;
if(i >= RX_LIST_SIZE) {
error_occurred(ERROR_CAN_RXLST_OF);
}
// ************* NOW, fill RTR descriptors ************************** //
i = 0;
//Add NULL terminator
rtrlist[i++] = (CAN_FRAME_DESC *) 0;
if(i >= RTR_LIST_SIZE) {
error_occurred(ERROR_CAN_RTRLST_OF);
}
// ************** NOW, fill standard non-RTR TX descriptors (not in a list) *** //
can_set_tx_descriptor_ii(&tx_fd_error, ID_ERROR_MCSO, CHAN_CAN1, error_get_info, error_get_time);
can_set_tx_descriptor_fi(&tx_fd_color_white, ID_MCSO_COLOR_BACK_WHITE, CHAN_CAN1, dn_get_color_white, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_color_red, ID_MCSO_COLOR_BACK_RED, CHAN_CAN1, dn_get_color_red, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_color_green, ID_MCSO_COLOR_BACK_GREEN, CHAN_CAN1, dn_get_color_green, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_color_blue, ID_MCSO_COLOR_BACK_BLUE, CHAN_CAN1, dn_get_color_blue, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_left_ankle_angle, ID_MCSO_LEFT_ANKLE_ANGLE, CHAN_CAN1, dn_get_ankle_pos_rads, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_left_ankle_rate, ID_MCSO_LEFT_ANKLE_RATE, CHAN_CAN1, dn_get_ankle_vel_rads, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_status, ID_MCSO_STATUS, CHAN_CAN1, dn_get_status, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_exec_time, ID_MCSO_EXECUTION_TIME, CHAN_CAN1, dn_get_execution_time, asched_get_timestamp);
can_set_tx_descriptor_fi(&tx_fd_max_exec_time, ID_MCSO_MAX_EXECUTION_TIME, CHAN_CAN1, dn_get_max_execution_time, asched_get_timestamp);
// ******** Set CAN rx descriptors ***********
can_rx_set_descriptors(rxlist, rtrlist);
// ******** Initialize low-level CAN RX struct for assembly function *********
can_init();
}
/**
Turns off the motor controller until it is safe to turn back on.
Sets the appropriate state and causes an error. Sets the PWM to 0 and
lets the watchdog timer starve out. Also turns on the green LED.
@param reason Why this function was called.
@private
*/
static void mc_turnoff(MC_SAFETY_CAUSE reason){
switch(reason){ //set appropriate state and cause error
case MC_THERM: mc_therm_off = 1;
error_occurred(ERROR_MC_TEMP_OFF);
break;
case MC_MECH: mc_mech_off = 1;
error_occurred(ERROR_MC_MECH_OFF);
break;
default: mc_off = 1;
error_occurred(ERROR_MC_SHUTOFF);
break;
}
mc_pwm = 0;
PWMMR2 = 0; //Both PWMs set to 0
PWMMR6 = 0;
PWMLER = (1<<6)|(1<<2);
mcu_led_green_on();
}
void repl_evaluate_line(Stack* context, std::string const& input, std::ostream& output)
{
Branch* branch = top_branch(context);
int previousHead = branch->length();
parser::compile(branch, parser::statement_list, input);
int newHead = branch->length();
bool anyErrors = false;
int resultIndex = -1;
for (int i=previousHead; i < newHead; i++) {
Term* result = branch->get(i);
if (has_static_error(result)) {
output << "error: ";
print_static_error(result, output);
output << std::endl;
anyErrors = true;
break;
}
Frame* frame = top_frame(context);
frame->pc = i;
frame->endPc = i + 1;
run_interpreter(context);
if (error_occurred(context)) {
output << "error: ";
print_error_stack(context, std::cout);
anyErrors = true;
break;
}
resultIndex = i;
}
// Print results of the last expression
if (!anyErrors && resultIndex != -1) {
Term* result = branch->get(resultIndex);
if (result->type != as_type(VOID_TYPE)) {
output << find_stack_value_for_term(context, result, 0)->toString() << std::endl;
}
}
clear_error(context);
}
/**
* Schedule the next tasks. ISR context!
*/
void sched_tick(void) {
int search_idx;
if (!mutex_check(&sched_mutex)){ //mutex is unlocked
mutex_lock(&sched_mutex);
if(sched_div_counter >= (sched_divider - 1)){
//1) check if all tasks have successfully completed from the previous iteration
if(sched_completion != SCHED_DONE){
error_occurred(ERROR_SCHED_OVERRUN, PRIORITY_HIGH);
}
//2) schedule next tasks
if(sched_next_task_index >= 0){
search_idx = sched_next_task_index;
while(sched_schedule[search_idx] != (TASK_PTR)NULL){//find end of current time slot in case of overrun
search_idx++;
}
search_idx++;//advance to first task of next slot
//if we have reached the end of the schedule (double NULL), wrap to the beginning
if(sched_schedule[search_idx] == (TASK_PTR)NULL){
search_idx = 0;
}
} else {
sched_status = SCHED_RUNNING_SYNC;
search_idx = 0; //first run of the scheduler
}
sched_slot_start_index = search_idx;//schedule the beginning of the next time slot
sched_completion = SCHED_NOT_DONE;
//3) reset divider
sched_div_counter = 0;
} else {
sched_div_counter++;
}
mutex_unlock(&sched_mutex);
} else {//mutex previously locked by some other process
//do nothing
}
}
/*
* Called by the auxiliary timer to provide
* backup ticks if asynchronous
*/
void sched_auxiliary_tick(){
sched_aux_count++;
sched_sync_count++;
if (sched_status == SCHED_NOT_RUNNING){
sched_status = SCHED_RUNNING_SYNC;
}
// ******** SYNC ********* //
if (sched_status == SCHED_RUNNING_SYNC){
//increase timestamp if match
if (sched_aux_count >= sched_aux_match){
sched_timestamp++;
sched_aux_count = 0;
} //in sync, don't expect sync yet, so sched_tick
if (sched_sync_count < sched_sync_match){
sched_aux_reset_timer();
sched_tick();
} //expected sync, go async and sched_tick
else if (sched_sync_count > sched_sync_match){
sched_aux_reset_timer();
error_occurred(ERROR_SCHED_ASYNC, PRIORITY_HIGH);
sched_status = SCHED_RUNNING_ASYNC;
sched_aux_count = 0;
sched_tick();
}
} //******* ASYNC *******//
else if (sched_status == SCHED_RUNNING_ASYNC){
sched_aux_reset_timer();
//increase timestamp if match
if (sched_aux_count >= sched_aux_match){
sched_timestamp++;
sched_aux_count = 0;
} //running async, so always generate tick
sched_tick();
}
}
static void *bg_new_img(void *data)
{
/* so we can poll for it */
block_usr1_signal();
struct imv_loader *ldr = data;
char path[PATH_MAX] = "-";
pthread_mutex_lock(&ldr->lock);
int from_stdin = !strncmp(path, ldr->path, 2);
if(!from_stdin) {
(void)snprintf(path, PATH_MAX, "%s", ldr->path);
}
pthread_mutex_unlock(&ldr->lock);
FREE_IMAGE_FORMAT fmt;
if (from_stdin) {
pthread_mutex_lock(&ldr->lock);
ldr->fi_buffer = FreeImage_OpenMemory(ldr->buffer, ldr->buffer_size);
fmt = FreeImage_GetFileTypeFromMemory(ldr->fi_buffer, 0);
pthread_mutex_unlock(&ldr->lock);
} else {
fmt = FreeImage_GetFileType(path, 0);
}
if(fmt == FIF_UNKNOWN) {
if (from_stdin) {
pthread_mutex_lock(&ldr->lock);
FreeImage_CloseMemory(ldr->fi_buffer);
pthread_mutex_unlock(&ldr->lock);
}
error_occurred(ldr);
return NULL;
}
int num_frames = 1;
FIMULTIBITMAP *mbmp = NULL;
FIBITMAP *bmp = NULL;
int width, height;
int raw_frame_time = 100; /* default to 100 */
if(fmt == FIF_GIF) {
if(from_stdin) {
pthread_mutex_lock(&ldr->lock);
mbmp = FreeImage_LoadMultiBitmapFromMemory(FIF_GIF, ldr->fi_buffer,
GIF_LOAD256);
pthread_mutex_unlock(&ldr->lock);
} else {
mbmp = FreeImage_OpenMultiBitmap(FIF_GIF, path,
/* don't create file */ 0,
/* read only */ 1,
/* keep in memory */ 1,
/* flags */ GIF_LOAD256);
}
if(!mbmp) {
error_occurred(ldr);
return NULL;
}
num_frames = FreeImage_GetPageCount(mbmp);
FIBITMAP *frame = FreeImage_LockPage(mbmp, 0);
width = FreeImage_GetWidth(frame);
height = FreeImage_GetHeight(frame);
bmp = FreeImage_ConvertTo32Bits(frame);
/* get duration of first frame */
FITAG *tag = NULL;
FreeImage_GetMetadata(FIMD_ANIMATION, frame, "FrameTime", &tag);
if(FreeImage_GetTagValue(tag)) {
raw_frame_time = *(int*)FreeImage_GetTagValue(tag);
}
FreeImage_UnlockPage(mbmp, frame, 0);
} else {
/* Future TODO: If we load image line-by-line we could stop loading large
* ones before wasting much more time/memory on them. */
int flags = (fmt == FIF_JPEG) ? JPEG_EXIFROTATE : 0;
FIBITMAP *image;
if(from_stdin) {
pthread_mutex_lock(&ldr->lock);
image = FreeImage_LoadFromMemory(fmt, ldr->fi_buffer, flags);
pthread_mutex_unlock(&ldr->lock);
} else {
image = FreeImage_Load(fmt, path, flags);
}
if(!image) {
error_occurred(ldr);
pthread_mutex_lock(&ldr->lock);
FreeImage_CloseMemory(ldr->fi_buffer);
ldr->fi_buffer = NULL;
pthread_mutex_unlock(&ldr->lock);
return NULL;
}
/* Check for cancellation before we convert pixel format */
if(is_thread_cancelled()) {
FreeImage_Unload(image);
return NULL;
}
width = FreeImage_GetWidth(bmp);
height = FreeImage_GetHeight(bmp);
bmp = FreeImage_ConvertTo32Bits(image);
FreeImage_Unload(image);
}
/* now update the loader */
pthread_mutex_lock(&ldr->lock);
/* check for cancellation before finishing */
if(is_thread_cancelled()) {
if(mbmp) {
FreeImage_CloseMultiBitmap(mbmp, 0);
}
if(bmp) {
FreeImage_Unload(bmp);
}
pthread_mutex_unlock(&ldr->lock);
return NULL;
}
if(ldr->mbmp) {
FreeImage_CloseMultiBitmap(ldr->mbmp, 0);
}
if(ldr->bmp) {
FreeImage_Unload(ldr->bmp);
}
ldr->mbmp = mbmp;
ldr->bmp = bmp;
if(ldr->out_bmp) {
FreeImage_Unload(ldr->out_bmp);
}
ldr->out_bmp = FreeImage_Clone(bmp);
ldr->out_is_new_image = 1;
ldr->width = width;
ldr->height = height;
ldr->cur_frame = 0;
ldr->next_frame = 1;
ldr->num_frames = num_frames;
ldr->frame_time = (double)raw_frame_time * 0.0001;
pthread_mutex_unlock(&ldr->lock);
return NULL;
}
/**
A dummy function with @c void inputs that returns a @c float.
Use this as the default for function pointers.
*/
float floatvoid(void){error_occurred(ERROR_UTIL_DUMMY);return 0.0;}
/**
A dummy function with @c void inputs that returns a @c int.
Use this as the default for function pointers.
*/
int intvoid(void){error_occurred(ERROR_UTIL_DUMMY);return 0;}
/**
Uses a PID controller to try and reach the target current.
The target currect used by this function should be set using
@ref mc_set_target_current or @ref mc_set_target_current_fixed.
The constants for the current controller are given by the @c kp, @c ki, and @c kd
given to @ref mc_init.
Updates the watchdog, and uses both a thermal limiting multiplier and integrating
current limits.
*/
void mc_pid_current(void)
{
volatile fixed mc_error = 0;
static unsigned char start_count = 2;
signed long int curr_pwm = 0;
long long int pid_sum, p_component, i_component;
fixed thermal_multiplier, mechanical_multiplier;
mc_update_watchdog(); //feed watchdog if running or sleeping
if (!start_count){ //wait a few iterations to allow the adcx to get meaningful data
fixed current = mc_data.get_current(); //motor current on the robot
fixed target_current = mc_target_current; //the target current
thermal_multiplier = mc_get_thermal_limiter(current); //thermal multiplier, est. temp and limit current
mechanical_multiplier = mc_smart_check_current(current);
//During direction control, we limit the target_current to the correct direction.
//Used to do PWM, but ran into problem with Integral control
if (target_current < 0) { //trying to go in negative direction
target_current = target_current * mc_negative;
} else if (target_current > 0) { //trying to go in positive direction, but not allowed
target_current = target_current * mc_positive;
}
if (mc_is_running())
{
// ******************************* calc prop and integral **************************
//Calculate the error:
mc_error = (target_current - current);
//Integral saturation during direction control is fixed by limiting the
//target_current during direction_control, not the PWM
mc_data.integral += mc_error;
// ****************** check for saturation of integral constant ********************
if (mc_data.integral > mc_data.max_integral){
mc_data.integral = mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
} else if (mc_data.integral < -1*mc_data.max_integral){
mc_data.integral = -1*mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
}
// Sum the terms of the PID algorithm to give the new PWM duty cycle value
//Note: Need to bound each component such that their sum can't overflow
p_component = fixed_mult_to_long((mc_error),(mc_data.kp));
i_component = fixed_mult_to_long((mc_data.integral),(mc_data.ki));
pid_sum = p_component + i_component;
// check for fixed point overflow
if(pid_sum > FIXED_MAX) {
pid_sum = FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
} else if (pid_sum < -1*FIXED_MAX) {
pid_sum = -1*FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
}
if(thermal_multiplier < mechanical_multiplier){ //use whichever multiplier is smaller
pid_sum = fixed_mult(pid_sum, thermal_multiplier);
} else {
pid_sum = fixed_mult(pid_sum, mechanical_multiplier);
}
curr_pwm = fixed_to_int((pid_sum));
// ********************** check if PWM is within limits ****************************
if (curr_pwm > mc_data.max_pwm){
curr_pwm = mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
else if (curr_pwm < -1*mc_data.max_pwm){
curr_pwm = -1*mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
mc_set_pwm(curr_pwm);
} else {
mc_set_pwm(0);
}
} else {
start_count--;
}
}
/**
A dummy function with an @c int input that returns @c void.
Use this as the default for function pointers.
@param a Dummy integer.
*/
void voidint(int a){error_occurred(ERROR_UTIL_DUMMY);}
void do_file_command(List* args, caValue* reply)
{
RawOutputPrefs rawOutputPrefs;
bool printRaw = false;
bool printSource = false;
bool printState = false;
bool dontRunScript = false;
int argIndex = 1;
while (true) {
if (argIndex >= args->length()) {
set_string(reply, "No filename found");
return;
}
if (string_eq(args->get(argIndex), "-p")) {
printRaw = true;
argIndex++;
continue;
}
if (string_eq(args->get(argIndex), "-pp")) {
printRaw = true;
rawOutputPrefs.showProperties = true;
argIndex++;
continue;
}
if (string_eq(args->get(argIndex), "-b") || string_eq(args->get(argIndex), "-pb")) {
printRaw = true;
rawOutputPrefs.showBytecode = true;
argIndex++;
continue;
}
if (string_eq(args->get(argIndex), "-s")) {
printSource = true;
argIndex++;
continue;
}
if (string_eq(args->get(argIndex), "-print-state")) {
printState = true;
argIndex++;
continue;
}
if (string_eq(args->get(argIndex), "-n")) {
dontRunScript = true;
argIndex++;
continue;
}
break;
}
Block block;
load_script(&block, as_cstring(args->get(argIndex)));
if (printSource)
std::cout << get_block_source_text(&block);
if (dontRunScript)
return;
Stack stack;
evaluate_block(&stack, &block);
if (printState)
std::cout << to_string(&stack.state) << std::endl;
if (error_occurred(&stack)) {
std::cout << "Error occurred:\n";
print_error_stack(&stack, std::cout);
std::cout << std::endl;
return;
}
}
/**
Returns whether the motor is running or not.
@return The status of the motor.
- @b 0: if the motor is off, asleep, or shutdown
- @b 1: otherwise
@private
*/
static int mc_is_running(void){
int running = (!mc_off) && (!mc_sleep) && (!mc_shutdown) && (!mc_therm_off) && (!mc_mech_off);
if (!running)error_occurred(ERROR_MC_NOT_RUNNING);
return running;
}
int
main(void)
{
assert_match("to be or not to be", "to be or not to be");
assert_no_match("to be or not to be ", "to be or not to be");
assert_no_match("jkjke", "jkjk");
assert_no_match("ijkjke", "ijkjk");
assert_no_match("iijkjke", "iijkjk");
assert_no_match("iiijkjke", "iiijkjk");
assert_no_match("iiiijkjke", "iiiijkjk");
assert_no_match("iiiiijkjke", "iiiiijkjk");
assert_no_match("iiiiiijkjke", "iiiiiijkjk");
assert_no_match("iiiiiiijkjke", "iiiiiiijkjk");
assert_no_match("iiiiiiiijkjke", "iiiiiiiijkjk");
assert_no_match("ab", "bb");
{
const uint8_t a[3] = { 0, 0, 0 };
const uint8_t b[5] = { 0, 0, 0, 0, 0 };
uint64_t left = seahash(a, 3 * sizeof(uint8_t));
uint64_t right = seahash(b, 5 * sizeof(uint8_t));
if (left == right) {
error_occurred("Hash was found to be zero sensitive");
}
}
{
const uint8_t a[4] = { 1, 2, 3, 4 };
const uint8_t b[5] = { 1, 0, 2, 3, 4 };
uint64_t left = seahash(a, 4 * sizeof(uint8_t));
uint64_t right = seahash(b, 5 * sizeof(uint8_t));
if (left == right) {
error_occurred("Hash was found to be zero sensitive");
}
}
{
const uint8_t a[4] = { 1, 2, 3, 4 };
const uint8_t b[6] = { 1, 0, 0, 2, 3, 4 };
uint64_t left = seahash(a, 4 * sizeof(uint8_t));
uint64_t right = seahash(b, 6 * sizeof(uint8_t));
if (left == right) {
error_occurred("Hash was found to be zero sensitive");
}
}
{
const uint8_t a[4] = { 1, 2, 3, 4 };
const uint8_t b[5] = { 1, 2, 3, 4, 0 };
uint64_t left = seahash(a, 4 * sizeof(uint8_t));
uint64_t right = seahash(b, 5 * sizeof(uint8_t));
if (left == right) {
error_occurred("Hash was found to be zero sensitive");
}
}
return EXIT_SUCCESS;
}
void ScintillaDocument::emit_error_occurred(int status) {
emit error_occurred(status);
}
/**
Sets the PWM signal going to the motor to @c pwm.
@param pwm The PWM going to the motor. @code -1*max_pwm < pwm < max_pwm @endcode
*/
void mc_set_pwm(int pwm)
{
const unsigned long int full = PWMMR0;
const unsigned long int offset = 14;
mc_update_watchdog();
if (!mc_is_running()){ //controller is asleep, shutdown, or off
pwm = 0;
}
//Shut off all H-bridge switches when in sleep mode
if (mc_sleep)
{
PWMMR2 = 0; //Set duty cycle of PWM2 (B) to 0
PWMMR6 = 0; //Set duty cycle of PWM6 (A) to 0
PWMLER = (1<<2)|(1<<6); //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
FIO0CLR = (1<<11); // turn off low side A enable output
FIO1CLR = (1<<17); // turn off low side B enable output
}
else //Set PWM normally and enable low-side switches
{
FIO0SET = (1<<11); // turn on low side A enable output
FIO1SET = (1<<17); // turn on low side B enable output
pwm = mc_data.operation * pwm;
mc_pwm = pwm;
//check bounds
if (pwm > mc_data.max_pwm)
{
pwm = mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
else if (pwm < -1*mc_data.max_pwm)
{
pwm = -1*mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
if (mc_phase == 0)
{ //Normal operation: PWMA = x%, PWMB = 0%
if(pwm < 0)
{ //Negative current, clockwise
PWMMR2 = 0; //Set duty cycle of PWM2 to 0 so that only one motor direction is on
PWMMR6 = -1 * pwm; //Set duty cycle of PWM6 (out of 600 max)
PWMLER = (1<<2)|(1<<6); //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
}
else
{ //Positive current, counter-clockwise
PWMMR2 = pwm; //Set duty cycle of PWM2 (out of 600 max)
PWMMR6 = 0; //Set duty cycle of PWM6 to 0 so that only one motor direction is on
PWMLER = (1<<2)|(1<<6); //Transfer new PWMMR2 & PWMMR6 values to reg at next PWM match
}
mc_phase = 1;
}
else
{ //Alternate Operation: PWMA = 100%, PWMB = 100-x%
if(pwm < 0) {//negative current, clockwise
PWMMR2 = full - (pwm * -1) - offset;
PWMMR6 = full;
PWMLER = (1<<2)|(1<<6);
} else { //positive current, counter-clockwise
PWMMR2 = full;
PWMMR6 = full - (pwm) - offset;
PWMLER = (1<<2)|(1<<6);
}
mc_phase = 0;
}
}
}
int run_command_line(caWorld* world, caValue* args)
{
RawOutputPrefs rawOutputPrefs;
bool printRaw = false;
bool printState = false;
bool dontRunScript = false;
bool printTrace = false;
// Prepended options
while (true) {
if (list_length(args) == 0)
break;
if (string_eq(list_get(args, 0), "-break-on")) {
DEBUG_BREAK_ON_TERM = atoi(as_cstring(list_get(args, 1)));
list_remove_index(args, 0);
list_remove_index(args, 0);
std::cout << "breaking on creation of term: " << DEBUG_BREAK_ON_TERM << std::endl;
continue;
}
if (string_eq(list_get(args, 0), "-path")) {
// Add a module path
module_add_search_path(world, as_cstring(list_get(args, 1)));
list_remove_index(args, 0);
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-p")) {
printRaw = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-pp")) {
printRaw = true;
rawOutputPrefs.showProperties = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-b") || string_eq(list_get(args, 0), "-pb")) {
printRaw = true;
rawOutputPrefs.showBytecode = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-n")) {
dontRunScript = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-print-state")) {
printState = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-t")) {
printTrace = true;
list_remove_index(args, 0);
continue;
}
if (string_eq(list_get(args, 0), "-load")) {
caValue* filename = list_get(args, 1);
Value moduleName;
module_get_default_name_from_filename(filename, &moduleName);
list_remove_index(args, 0);
list_remove_index(args, 0);
continue;
}
break;
}
// No arguments remaining
if (list_length(args) == 0) {
print_usage();
return 0;
}
Block* mainBlock = fetch_module(world, "main");
// Check to handle args[0] as a dash-command.
// Print help
if (string_eq(list_get(args, 0), "-help")) {
print_usage();
return 0;
}
// Eval mode
if (string_eq(list_get(args, 0), "-e")) {
list_remove_index(args, 0);
Value command;
set_string(&command, "");
bool firstArg = true;
while (!list_empty(args)) {
if (!firstArg)
string_append(&command, " ");
string_append(&command, list_get(args, 0));
list_remove_index(args, 0);
firstArg = false;
}
caValue* result = term_value(mainBlock->eval(as_cstring(&command)));
std::cout << to_string(result) << std::endl;
return 0;
}
// Start repl
if (string_eq(list_get(args, 0), "-repl")) {
run_repl_stdin(world);
return 0;
}
if (string_eq(list_get(args, 0), "-call")) {
Symbol loadResult = load_script(mainBlock, as_cstring(list_get(args, 1)));
if (loadResult == sym_Failure) {
std::cout << "Failed to load file: " << as_cstring(list_get(args, 1)) << std::endl;
return -1;
}
block_finish_changes(mainBlock);
caStack* stack = circa_alloc_stack(world);
// Push function
caBlock* func = circa_find_function_local(mainBlock, as_cstring(list_get(args, 2)));
circa_push_function(stack, func);
// Push inputs
for (int i=3, inputIndex = 0; i < circa_count(args); i++) {
caValue* val = circa_input(stack, inputIndex++);
circa_parse_string(as_cstring(list_get(args, i)), val);
}
circa_run(stack);
if (circa_has_error(stack)) {
circa_print_error_to_stdout(stack);
}
// Print outputs
for (int i=0;; i++) {
caValue* out = circa_output(stack, i);
if (out == NULL)
break;
std::cout << to_string(circa_output(stack, i)) << std::endl;
}
circa_dealloc_stack(stack);
}
// Start debugger repl
if (string_eq(list_get(args, 0), "-d"))
return run_debugger_repl(as_cstring(list_get(args, 1)));
// Generate cpp headers
if (string_eq(list_get(args, 0), "-gh")) {
load_script(mainBlock, as_cstring(list_get(args, 1)));
std::cout << generate_cpp_headers(mainBlock);
return 0;
}
// Run file checker
if (string_eq(list_get(args, 0), "-check"))
return run_file_checker(as_cstring(list_get(args, 1)));
// Export parsed information
if (string_eq(list_get(args, 0), "-export")) {
const char* filename = "";
const char* format = "";
if (list_length(args) >= 2)
format = as_cstring(list_get(args, 1));
if (list_length(args) >= 3)
filename = as_cstring(list_get(args, 2));
return run_exporting_parser(format, filename);
}
// Build tool
if (string_eq(list_get(args, 0), "-build")) {
return run_build_tool(args);
}
// C++ gen
if (string_eq(list_get(args, 0), "-cppgen")) {
Value remainingArgs;
list_slice(args, 1, -1, &remainingArgs);
run_generate_cpp(&remainingArgs);
return 0;
}
// Command reader (from stdin)
if (string_eq(list_get(args, 0), "-run-stdin")) {
run_commands_from_stdin();
return 0;
}
// Reproduce source text
if (string_eq(list_get(args, 0), "-source-repro")) {
load_script(mainBlock, as_cstring(list_get(args, 1)));
std::cout << get_block_source_text(mainBlock);
return 0;
}
// Rewrite source, this is useful for upgrading old source
if (string_eq(list_get(args, 0), "-rewrite-source")) {
load_script(mainBlock, as_cstring(list_get(args, 1)));
std::string contents = get_block_source_text(mainBlock);
write_text_file(as_cstring(list_get(args, 1)), contents.c_str());
return 0;
}
// Default behavior with no flags: run args[0] as a script filename.
// Add the script's folder to module search paths.
caValue* filename = list_get(args, 0);
Value directory;
get_directory_for_filename(filename, &directory);
module_add_search_path(world, as_cstring(&directory));
load_script(mainBlock, as_cstring(filename));
block_finish_changes(mainBlock);
refresh_bytecode(mainBlock);
if (printRaw)
print_block(mainBlock, &rawOutputPrefs, std::cout);
if (dontRunScript)
return 0;
Stack* stack = alloc_stack(world);
push_frame(stack, mainBlock);
run_interpreter(stack);
if (printState)
std::cout << to_string(&stack->state) << std::endl;
if (error_occurred(stack)) {
std::cout << "Error occurred:\n";
print_error_stack(stack, std::cout);
std::cout << std::endl;
std::cout << "Stack:\n";
dump(stack);
return 1;
}
return 0;
}
/**
A dummy function with @c void inputs that returns @c void.
Use this as the default for function pointers.
*/
void voidvoid(void){error_occurred(ERROR_UTIL_DUMMY);}
