void motion_hold(int time)
{
float errorRight, errorLeft;
float rightOutput, leftOutput;
elapsedMicros currentTime;
PIDCorrectionCalculator* left_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
PIDCorrectionCalculator* right_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
currentTime = 0;
while (currentTime / 1000 < time) {
errorLeft = enc_left_extrapolate();
errorRight = enc_right_extrapolate();
leftOutput = left_PID_calculator->Calculate(errorLeft);
rightOutput = right_PID_calculator->Calculate(errorRight);
motor_set(&motor_a, leftOutput);
motor_set(&motor_b, rightOutput);
}
motor_set(&motor_a, 0);
motor_set(&motor_b, 0);
}
void keydo(uint8 keyc)
{
static bit t=0;
if(keyc<=0x39&&keyc>=0x30)
{
if(t==0)
motor_set((keyc-'0')*360,0);
else motor_set((keyc-'0')*360,1);
}
if(keyc==0x1b)
{
motorstop();
}
if(keyc==0x25)
{
t=1;
}
if(keyc==0x27)
{
t=0;
}
if(keyc==0x26)
{
motor_set(90,0);
}
if(keyc==0x28)
{
motor_set(90,1);
}
if(keyc==0x0D)
{
motor_set(99999999,t);
}
}
void motor_set_all(int16_t speed)
{
motor_set(MOTOR_1, speed);
motor_set(MOTOR_2, speed);
motor_set(MOTOR_3, speed);
motor_set(MOTOR_4, speed);
}
void motion_forward(float distance, float exit_speed) {
float errorRight, errorLeft;
float rightOutput, leftOutput;
float idealDistance, idealVelocity;
float forcePerMotor;
elapsedMicros moveTime;
motionCalc motionCalc (distance, max_vel_straight, exit_speed, max_accel_straight, max_decel_straight);
PIDCorrectionCalculator* left_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
PIDCorrectionCalculator* right_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
// zero clock before move
moveTime = 0;
// execute motion
while (idealDistance != distance) {
//Run sensor protocol here. Sensor protocol should use encoder_left/right_write() to adjust for encoder error
idealDistance = motionCalc.idealDistance(moveTime);
idealVelocity = motionCalc.idealVelocity(moveTime);
errorLeft = enc_left_extrapolate() - idealDistance;
errorRight = enc_right_extrapolate() - idealDistance;
// use instantaneous velocity of each encoder to calculate what the ideal PWM would be
if (idealVelocity > 0) {
forcePerMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forcePerMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
leftOutput = left_motor_output_calculator.Calculate(forcePerMotor, idealVelocity);
rightOutput = right_motor_output_calculator.Calculate(forcePerMotor, idealVelocity);
//run PID loop here. new PID loop will add or subtract from a predetermined PWM value that was calculated with the motor curve and current ideal speed
// add PID error correction to ideal value
leftOutput += left_PID_calculator->Calculate(errorLeft);
rightOutput += right_PID_calculator->Calculate(errorRight);
//Serial2.println(errorLeft);
// set motors to run at specified rate
motor_set(&motor_a, leftOutput);
motor_set(&motor_b, rightOutput);
}
enc_left_write(0);
enc_right_write(0);
//String stuff
//Serial2.printf("%s", outputString.c_str());
}
static void event(int evt)
{
switch (evt) {
case EVT_FRONT_BLOCKED:
puts("front blocked");
break;
case EVT_BACK_BLOCKED:
puts("back blocked");
break;
case EVT_FRONT_FREE:
puts("front free");
break;
case EVT_BACK_FREE:
puts("back free");
break;
default:
puts("unkown");
}
if ((behavior == 0) || (behavior == 1)) {
/* start condition */
if ((state == 0) && (evt == EVT_FRONT_FREE)) {
puts("evt: start");
speed = CONF_BRAIN_SPEED;
motor_set(&mot, speed);
}
/* going forward, hitting obstacle -> steer full and go backwards */
else if ((state == 0) && (evt == EVT_FRONT_BLOCKED)) {
motor_stop(&mot);
puts("evt: hit forward obstacle");
dir = (behavior == 0) ? -1000 : 1000;
speed = -CONF_BRAIN_SPEED;
brain_steer(dir);
motor_set(&mot, speed);
state = 1;
}
/* going backwards, hitting obstacle */
else if ((state == 1) && (evt == EVT_BACK_BLOCKED)) {
motor_stop(&mot);
puts("evt: hit backwards obstacle");
dir = 0;
speed = CONF_BRAIN_SPEED;
brain_steer(dir);
motor_set(&mot, speed);
state = 0;
}
}
}
// clockwise angle is positive, angle is in degrees
void motion_rotate(float angle) {
float distancePerDegree = 3.14159265359 * MM_BETWEEN_WHEELS / 360;
float idealLinearDistance, idealLinearVelocity;
float errorLeft, errorRight;
float linearDistance = distancePerDegree * angle;
float forcePerMotor;
float rightOutput, leftOutput;
elapsedMicros moveTime;
motionCalc motionCalc (linearDistance, max_vel_rotate, 0, max_accel_rotate, max_decel_rotate);
PIDCorrectionCalculator* left_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
PIDCorrectionCalculator* right_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
// zero encoders and clock before move
moveTime = 0;
// the right will always be the negative of the left in order to rotate on a point.
while (idealLinearDistance != linearDistance) {
//Run sensor protocol here. Sensor protocol should use encoder_left/right_write() to adjust for encoder error
idealLinearDistance = motionCalc.idealDistance(moveTime);
idealLinearVelocity = motionCalc.idealVelocity(moveTime);
errorLeft = enc_left_extrapolate() - idealLinearDistance;
errorRight = enc_right_extrapolate() + idealLinearDistance;
if (idealLinearVelocity > 0) {
forcePerMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forcePerMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
leftOutput = left_motor_output_calculator.Calculate(forcePerMotor, idealLinearVelocity);
rightOutput = right_motor_output_calculator.Calculate(-forcePerMotor, idealLinearVelocity);
leftOutput += left_PID_calculator->Calculate(errorLeft);
rightOutput += right_PID_calculator->Calculate(errorRight);
// set motors to run at specified rate
motor_set(&motor_a, leftOutput);
motor_set(&motor_b, rightOutput);
//run PID loop here. new PID loop will add or subtract from a predetermined PWM value that was calculated with the motor curve and current ideal speed
}
motor_set(&motor_a, 0);
motor_set(&motor_b, 0);
enc_left_write(0);
enc_right_write(0);
}
void drive_set(int16_t vel, int16_t dir)
{
int32_t l = vel - dir;
int32_t r = vel + dir;
int32_t max = MAX(ABS(l),ABS(r));
if (max > MOTOR_SPEED_MAX) {
l = (l * MOTOR_SPEED_MAX) / max;
r = (r * MOTOR_SPEED_MAX) / max;
}
motor_set(DR_MTR_L, l);
motor_set(DR_MTR_R, r);
}
/**
* @brief Handles SPI for the motor set command.
*/
static void spis_cmd_motorset (void)
{
int16_t mota, motb;
uint8_t c = SPDR;
/* Still processing input. */
if (spis_pos < 4) {
/* Fill buffer. */
spis_buf[ spis_pos++ ] = c;
/* Echo recieved. */
SPDR = c;
/* Update CRC. */
spis_crc = _crc_ibutton_update( spis_crc, c );
}
/* Handle command. */
else {
/* Check CRC. */
if (c != spis_crc) {
SPIS_CMD_RESET();
LED0_ON();
return;
}
/* Prepare arguments. */
mota = (spis_buf[0]<<8) + spis_buf[1];
motb = (spis_buf[2]<<8) + spis_buf[3];
/* Set motor. */
motor_set( mota, motb );
/* Clear command. */
SPIS_CMD_RESET();
LED0_OFF();
}
}
/**
* Faehrt den Bot sauber herunter
*/
void ctbot_shutdown(void) {
LOG_INFO("Shutting c't-Bot down...");
motor_set(BOT_SPEED_STOP, BOT_SPEED_STOP);
#ifdef MAP_AVAILABLE
map_flush_cache();
#endif
#ifdef LOG_MMC_AVAILABLE
log_flush();
#endif
#ifdef BOT_FS_AVAILABLE
botfs_close_volume();
#endif
#ifdef DISPLAY_AVAILABLE
display_clear();
display_cursor(1, 1);
display_puts("SYSTEM HALTED.");
#endif // DISPLAY_AVAILABLE
ENA_off(0xff);
ctbot_shutdown_low();
}
void ICACHE_FLASH_ATTR
motor_object_unpack(PARAMS* params) {
struct motor* motor = create_motor();
motor->speed = get_next_int8(params);
motor_set(motor);
delete_motor(motor);
}
void brain_change_behavior(int bval)
{
behavior = bval;
if (((bval == 0) || (bval == 1)) && (speed == 0)) {
motor_set(&mot, CONF_BRAIN_SPEED);
}
else if (bval == 2) {
motor_stop(&mot);
}
}
void brain_change_behavior(int bval)
{
printf("[brain] new behavior: %i\n", bval);
behavior = bval;
if ((bval == 0) || (bval == 1)) {
state = 0;
motor_set(&mot, CONF_BRAIN_SPEED);
event(EVT_FRONT_FREE);
}
else if (bval == 2) {
motor_stop(&mot);
printf("[brain] STOPPING MOTOR\n");
}
}
/** Simple test program that pulses the PWM
channels so that it is obvious if it works.
An LED is on PB6 for debugging.
@param none
@return none */
void motor_test(void)
{
long i = 0;
short j = -255;
short k = 1;
short x = 59;
short y = 1;
DDRB |= (1<<PB2);
while(1)
{
// set the PWM duty cycle.
motor_set(j, x);
// delay loop
for (i = 0; i < 10000; i++)
{
continue;
}
//
if (j == 255)
{
k = -1;
}
else if (j == -255)
{
k = 1;
}
//
if (x == 255)
{
y = -1;
}
else if (x == -255)
{
y = 1;
}
// increment duty cycle
j += k;
x += y;
PORTB ^= (1<<PB2);
}
}
//Disable some component
void state_power_saving_init()
{
sensor_board_heater_on(false);
motor_set(0);
led_set(0,0,0);
//Disable TWI, SPI,Timer2 Timer1 ADC
PRR0=(1 <<PRTWI) | (1 << PRTIM2) | (1 << PRTIM0) | (1 << PRTIM1) | (1 << PRSPI) | (1 << PRADC) ;
//Disable Timer3, USART1
PRR1=(1 << PRTIM3) | (1 << PRUSART1);
//Disable Analog Comparator Disable
ACSR|= (1<<ACD);
//Enter in sleep mode
SMCR=(1 << SM1) | (1 << SE);
Delay_MS(200);
sleep();
}
void simple_autonomous(){
wait1Msec(500);
// on close edge of tile drive_inches(14.5); // works
drive_inches(14.5);
// touching wall drive_inches(28.5); // works
wait1Msec(500);
rotate_degrees_right(90); // works
wait1Msec(500);
reverse_until_bumpers(); // WOULD BE F*****G AWESOME IF ROBOTC HAD FUNCTION POINTERS!
// stay put for 4 seconds while the hook lowers!
wait1Msec(4000);
// drive_inches_slow(2);
motor_set(0, 0);
// do nothing; mission accomplised
// print to LCD for bonus points?
}
/** calls motor_set and sets each motor speed
to zero.
@param none
@return none */
void motor_stop(void)
{
motor_set(0,0);
}
task usercontrol() {
while (true) {
// Drive motors
motor_set(vexRT[Ch2], vexRT[Ch3]);
}
}
void motion_corner(float angle, float radius, float exit_speed) {
float errorRight, errorLeft;
float rightOutput, leftOutput;
float leftFraction, rightFraction;
float idealDistance, idealVelocity;
float forceLeftMotor, forceRightMotor;
float distance;
if (radius < 0) {
radius *= -1;
}
else if (radius == 0) {
motion_rotate(angle);
return;
}
if (exit_speed < 0) {
exit_speed *= -1;
}
distance = angle * 3.14159265359 * radius / 180;
if (distance < 0) {
distance *= -1;
}
elapsedMicros moveTime;
if (angle <= 0) {
leftFraction = (radius + MM_BETWEEN_WHEELS / 2) / radius;
rightFraction = (radius - MM_BETWEEN_WHEELS / 2) / radius;
}
else {
leftFraction = (radius - MM_BETWEEN_WHEELS / 2) / radius;
rightFraction = (radius + MM_BETWEEN_WHEELS / 2) / radius;
}
motionCalc motionCalc (distance, max_vel_corner, exit_speed, max_accel_corner, max_accel_corner);
PIDCorrectionCalculator* left_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
PIDCorrectionCalculator* right_PID_calculator = new PIDCorrectionCalculator(KP_POSITION,KI_POSITION,KD_POSITION);
// zero clock before move
moveTime = 0;
// execute motion
while (idealDistance != distance) {
//Run sensor protocol here. Sensor protocol should use encoder_left/right_write() to adjust for encoder error
idealDistance = motionCalc.idealDistance(moveTime);
idealVelocity = motionCalc.idealVelocity(moveTime);
errorLeft = enc_left_extrapolate() - idealDistance * rightFraction;
errorRight = enc_right_extrapolate() - idealDistance * leftFraction;
// use instantaneous velocity of each encoder to calculate what the ideal PWM would be
if (idealVelocity * leftFraction > 0) {
forceLeftMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * leftFraction + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forceLeftMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * leftFraction - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
if (idealVelocity * rightFraction > 0) {
forceRightMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * rightFraction + FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
else {
forceRightMotor = (ROBOT_MASS * motionCalc.idealAccel(moveTime) * rightFraction - FRICTION_FORCE) / NUMBER_OF_MOTORS; // this is acceleration motors should have at a current time
}
leftOutput = left_motor_output_calculator.Calculate(forceLeftMotor, idealVelocity * leftFraction);
rightOutput = right_motor_output_calculator.Calculate(forceRightMotor, idealVelocity * rightFraction);
//run PID loop here. new PID loop will add or subtract from a predetermined PWM value that was calculated with the motor curve and current ideal speed
// add PID error correction to ideal value
leftOutput += left_PID_calculator->Calculate(errorLeft);
rightOutput += right_PID_calculator->Calculate(errorRight);
// set motors to run at specified rate
motor_set(&motor_a, leftOutput);
motor_set(&motor_b, rightOutput);
}
enc_left_write(0);
enc_right_write(0);
}
void state_normal_play(){
// HACK do not allow all of these during posting until a clean way of aborting the FSM is coded.
if (post_res == NOT_POSTING && usb_serial_get_nb_received() > 0){
uint8_t res;
char t[32];
uint8_t c = usb_serial_get_byte();
switch(c){
case 'w':
#ifdef WIFI
state_set_next(&main_fsm, &state_config);
#endif
return;
break;
case 'e':
DCALIB_PRINTF("HUM calib : %u\r\n", sensor_calib_HUM_is_done());
DCALIB_PRINTF("TEMP calib : %u\r\n", sensor_calib_TEMP_is_done());
DCALIB_PRINTF("PM calib : %u\r\n", sensor_calib_PM_is_done());
DCALIB_PRINTF("k_HUM : %u\r\n", eeprom_read_byte(calib_eeprom_k_hum_addr));
DCALIB_PRINTF("b_HUM : %li\r\n", (uint32_t) round(eeprom_read_float(calib_eeprom_b_hum_addr)*100));
DCALIB_PRINTF("k_TEMP : %u\r\n", eeprom_read_byte(calib_eeprom_k_temp_addr));
DCALIB_PRINTF("b_TEMP : %li\r\n", (int32_t) round(eeprom_read_float(calib_eeprom_b_temp_addr)*100));
DCALIB_PRINTF("k_PM : %u\r\n", eeprom_read_byte(calib_eeprom_k_pm_addr));
DCALIB_PRINTF("b_PM : %li\r\n", (int32_t) round(eeprom_read_float(calib_eeprom_b_pm_addr)*100));
break;
case 'm':
fprintf(&USBSerialStream, "MAC: %s\r\n", get_mac_str());
break;
case 'g':
#ifdef CALIBRATION
sensor_calib_erase_calibration_k();
#endif
break;
case 'c':
#ifdef CALIBRATION
state_set_next(&main_fsm,&state_calibration);
#endif
break;
case '-':
println("Mem clear");
wifi_config_mem_clear();
error_set_post(POST_ERROR_WIFI_NO_CONFIG);
default:
break;
}
}
if (error_post() != POST_ERROR_WIFI_NO_CONFIG){
util_timer_enable_interrupt();
// TODO update motor
motor_set(180);// TEMP
prev_post_res = post_res;
// TODO integrate needs_to_post_room_changed to the flow to tell the backend that the room has changed...
post_res = post_fsm(); // this runs the fsm until it stops then doesn't do anything until it's reset.
uint32_t now = millis();
if (prev_post_res == POSTING && post_res == NOT_POSTING){ // just finished posting
if (post_fsm_error()){
error_set_post(POST_ERROR_WIFI_NO_POST);
} else {
error_set_post(POST_ERROR_NO_ERROR);
}
DSENSOR_INFO_PRINTF("%lu, VOC, VOC_RAW, CO2, VZ87_VOC, VZ87_CO2, VZ87_RAW, SENSOR_1, SENSOR_2, PM*100, PM_Cal*100, TMP*100,PMP_Cal*100, HUM*100, HUM_Cal*100\r\n", now);
sensor_update();
update_timeout_end = set_timeout(now + SENSOR_UPDATE_PERIOD_MS);
needs_to_post_room_changed = 0;
}
// when a change of room is detected, the timer before next post is put to at least
// 1 min to avoid posting old data to the new room.
#ifdef UPSIDE_DOWN
if ((post_res == NOT_POSTING) && upsidedown_detected ){ // posting must not be happening right now and an upside down condition must have been detected
upsidedown_detected = 0;
post_timeout_end = MAX(post_timeout_end, set_timeout(now + 60000));
needs_to_post_room_changed = 1;
}
#endif
#ifdef DOUBLE_TAP_ENABLED
if ((post_res == NOT_POSTING) && double_tap_detected){
reset_fsm();
}
#endif
if ( now > post_timeout_end && (post_res == NOT_POSTING)){
reset_fsm();
post_timeout_end = set_timeout(now + POST_PERIOD_MS);
} else if (now > update_timeout_end && (post_res == NOT_POSTING)){
sensor_update();
update_timeout_end = set_timeout(now + SENSOR_UPDATE_PERIOD_MS);
}
} else {
// The wifi has not been configured. This case is special since it makes it impossible to do anything interesting,
// aside from configuring it.
util_timer_disable_interrupt();
led_set(0, 0, 100);
}
}
/*!
* Der Roboter aktualisiert kontinuierlich seine Karte
* @param *data der Verhaltensdatensatz
*/
void bot_scan_onthefly_behaviour(Behaviour_t * data) {
static int16_t last_location_x, last_location_y;
static int16_t last_dist_x, last_dist_y, last_dist_head;
static int16_t last_border_x, last_border_y, last_border_head;
static uint8_t index = 0;
(void) data; // kein warning
/* Verhalten je nach Cache-Fuellstand */
uint8_t cache_free = (uint8_t) (map_update_fifo.size - map_update_fifo.count);
if (cache_free < SCAN_OTF_CACHE_LEVEL_THRESHOLD) {
if (cache_free == 1) {
/* Cache ganz voll */
if (scan_otf_modes.data.map_mode &&
sensBorderL < BORDER_DANGEROUS && sensBorderR < BORDER_DANGEROUS) {
/* Stoppe den Bot, damit wir Zeit haben die Karte einzutragen
* aber nur, wenn kein Abgrund erkannt wurde */
motor_set(BOT_SPEED_STOP, BOT_SPEED_STOP);
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("Map-Cache voll, halte Bot an");
#endif
/* Halte alle Verhalten eine Weile an, weil sie ja sonst evtl. weiterfahren wuerden */
os_thread_sleep(SCAN_OTF_SLEEP_TIME);
} else {
/* Cache voll, neuen Eintrag verwerfen */
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("Map-Cache voll, verwerfe neuen Eintrag");
#endif
}
return;
}
/* Cache sehr voll */
if (v_enc_left == 0 && v_enc_right == 0) {
/* Falls Bot gerade steht, dann kleine Pause */
os_thread_sleep(SCAN_OTF_SLEEP_TIME);
return;
}
}
/* Cache updaten, falls sich der Bot weit genug bewegt hat. */
index++;
if (index == MAP_UPDATE_CACHE_SIZE) {
index = 0;
}
map_cache_t * cache_tmp = &map_update_cache[index];
cache_tmp->mode.raw = 0;
cache_tmp->dataL = 0;
cache_tmp->dataR = 0;
#ifdef MEASURE_POSITION_ERRORS_AVAILABLE
cache_tmp->loc_prob = (uint8_t) (pos_error_radius < MAP_MAX_ERROR_RADIUS ? 255 - (pos_error_radius * 256
/ MAP_MAX_ERROR_RADIUS) : 0);
#endif // MEASURE_POSITION_ERRORS_AVAILABLE
/*
* STANDFLAECHE
* Die Standflaeche tragen wir nur ein, wenn der Bot auch ein Stueck gefahren ist
*/
if (scan_otf_modes.data.location) {
// ermitteln, wie weit der Bot seit dem letzten Location-Update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_location_x, last_location_y);
if (diff > (SCAN_OTF_RESOLUTION_DISTANCE_LOCATION * SCAN_OTF_RESOLUTION_DISTANCE_LOCATION)) {
// ist er weiter als SCAN_ONTHEFLY_DIST_RESOLUTION gefahren ==> Standflaeche aktualisieren
cache_tmp->mode.data.location = 1;
// Letzte Location-Update-Position sichern
last_location_x = x_pos;
last_location_y = y_pos;
}
}
/*
* DISTANZSENSOREN
* Die Distanzsensoren tragen wir beim Geradeausfahren selten ein,
* da sie viele Map-zellen ueberstreichen und das Eintragen teuer ist
* und sie auf der anderen Seite (beim Vorwaertsfahren) wenig neue Infos liefern
*/
if (scan_otf_modes.data.distance) {
// ermitteln, wie weit der Bot gedreht hat
int16_t turned = turned_angle(last_dist_head);
// ermitteln, wie weit der Bot seit dem letzten distance-update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_dist_x, last_dist_y);
if ((turned > SCAN_OTF_RESOLUTION_ANGLE_DISTSENS) ||
(diff > (SCAN_OTF_RESOLUTION_DISTANCE_DISTSENS * SCAN_OTF_RESOLUTION_DISTANCE_DISTSENS))) {
// Hat sich der Bot mehr als SCAN_ONTHEFLY_ANGLE_RESOLUTION gedreht ==> Blickstrahlen aktualisieren
cache_tmp->mode.data.distance = 1;
cache_tmp->dataL = (uint8_t) (sensDistL / 5);
cache_tmp->dataR = (uint8_t) (sensDistR / 5);
// Letzte Distanz-Update-Position sichern
last_dist_x = x_pos;
last_dist_y = y_pos;
last_dist_head = heading_int;
}
}
/*
* ABGRUNDSENSOREN
* Wir werten diese nur aus, wenn der Bot entweder
* SCAN_OTF_RESOLUTION_DISTANCE_BORDER mm gefahren ist oder
* SCAN_OTF_RESOLUTION_ANGLE_BORDER Grad gedreht hat
*/
if (scan_otf_modes.data.border) {
// ermitteln, wie weit der Bot seit dem letzten border-update gefahren ist
uint16_t diff = (uint16_t) get_dist(x_pos, y_pos, last_border_x, last_border_y);
// ermitteln, wie weit der Bot gedreht hat
int16_t turned = turned_angle(last_border_head);
if (((diff > (SCAN_OTF_RESOLUTION_DISTANCE_BORDER * SCAN_OTF_RESOLUTION_DISTANCE_BORDER)) || (turned > SCAN_OTF_RESOLUTION_ANGLE_BORDER))
&& ((sensBorderL > BORDER_DANGEROUS) || (sensBorderR > BORDER_DANGEROUS))) {
cache_tmp->mode.data.border = 1;
cache_tmp->mode.data.distance = 0;
cache_tmp->dataL = (uint8_t) (sensBorderL > BORDER_DANGEROUS);
cache_tmp->dataR = (uint8_t) (sensBorderR > BORDER_DANGEROUS);
last_border_x = x_pos;
last_border_y = y_pos;
last_border_head = heading_int;
}
}
// ist ein Update angesagt?
if (cache_tmp->mode.data.distance || cache_tmp->mode.data.location || cache_tmp->mode.data.border) {
cache_tmp->x_pos = x_pos;
cache_tmp->y_pos = y_pos;
#ifdef MAP_USE_TRIG_CACHE
if (cache_tmp->mode.data.distance || cache_tmp->mode.data.border) {
cache_tmp->sin = heading_sin;
cache_tmp->cos = heading_cos;
}
#else
cache_tmp->heading = heading_10_int;
#endif // MAP_USE_TRIG_CACHE
#ifdef DEBUG_SCAN_OTF
LOG_DEBUG("neuer Eintrag: x=%d y=%d head=%f distance=%d loaction=%d border=%d", cache_tmp->x_pos, cache_tmp->y_pos, cache_tmp->heading / 10.0f, cache_tmp->mode.distance, cache_tmp->mode.location, cache_tmp->mode.border);
#endif
_inline_fifo_put(&map_update_fifo, index, False);
}
}
static void run(void) {
if(ir_recv()) {
// receive a ir signal, react
motor_on();
led_on(RIGHT);
set_note(NOTE_B, 5);
wait_ms(200);
set_note(NOTE_F, 5);
wait_ms(100);
led_off(RIGHT);
led_on(LEFT);
set_note(NOTE_G, 5);
wait_ms(100);
set_note(NOTE_Ab, 5);
wait_ms(100);
set_note(NOTE_A, 5);
wait_ms(100);
led_off(LEFT);
motor_off();
} else if(button_clicked(RIGHT)) {
// button clicked, send ir signal and do some stuff
led_on(RIGHT);
set_note(NOTE_A, 5);
wait_ms(100);
set_note(NOTE_Ab, 5);
wait_ms(100);
set_note(NOTE_G, 5);
wait_ms(100);
led_off(RIGHT);
led_on(LEFT);
set_note(NOTE_F, 5);
wait_ms(100);
set_note(NOTE_B, 5);
wait_ms(200);
stop_note();
led_off(LEFT);
ir_on();
wait_ms(400);
ir_off();
wait_ms(10);
} else {
// regular bug behaviour
uint8_t light = photons_measure();
pentatonic_all_led_set(light >> 3);
motor_set(biased_random(light) > BASELINE + 0x40);
led_set(RIGHT, biased_random(light) > BASELINE + 0x00);
led_set(LEFT, biased_random(light) > BASELINE + 0x00);
if(biased_random(light) > BASELINE + 0x20) {
uint16_t tone = (biased_random(light) * 2) + 500;
set_note(tone, 0);
} else {
stop_note();
}
wait_ms(200);
}
}
void brain_set_speed(int16_t val)
{
motor_set(&mot, val);
}
int main(void)
{
system_init();
clock_init();
led_init(); led_set(0x01); //show life
UART_Init(BAUD); UART_Write("\nInit"); //Show UART life
motor_init();
adc_init();
//Enable Analog pins
adc_enable(CHANNEL_SENSOR_LEFT);
adc_enable(CHANNEL_SENSOR_RIGHT);
adc_enable(CHANNEL_SENSOR_FRONT);
//Sensor value variables
uint16_t sensor_left_value = 0; uint16_t sensor_right_value = 0; uint16_t sensor_front_value = 0;
//Analog inputSignal conditioning arrays
circBuf_t left_buffer; circBuf_t right_buffer; circBuf_t front_buffer;
//Initialise sensor averaging buffers
initCircBuf(&left_buffer, ROLLING_AVERAGE_LENGTH);
initCircBuf(&right_buffer, ROLLING_AVERAGE_LENGTH);
initCircBuf(&front_buffer, ROLLING_AVERAGE_LENGTH);
//UART output buffer
char buffer[UART_BUFF_SIZE] = {0};
//=====Application specific variables===== //TODO: initialise circbuff
circBuf_t sweep_times;
initCircBuf(&sweep_times, SWEEP_TIME_MEMORY_LENGTH);
short sweep_del_t_last = 0;
short sweep_end_t_last = 0;
//time when front sensor begins to see grey.
uint32_t grey_time_start = 0;
bool sweep_ended = FALSE;
//set high if the front sensor crosses the line
bool front_crossed_black = FALSE;
//set high if front finds finish line
bool front_crossed_grey = FALSE;
bool sensor_update_serviced = TRUE;
action current_action = IDLE;
int16_t forward_speed = DEFAULT_FORWARD_SPEED;
int16_t turn_speed = DEFAULT_SPEED;
//Scheduler variables
uint32_t t = 0;
//Loop control time variables
uint32_t maze_logic_t_last = 0;
uint32_t sample_t_last = 0;
uint32_t UART_t_last = 0;
clock_set_ms(0);
sei(); // Enable all interrupts
UART_Write("ialized\n");
//wait for start command
DDRD &= ~BIT(7);
PORTD |= BIT(7);
//motor_set(128, 128);
while((PIND & BIT(7)))
{
continue;
}
while(1)
{
t = clock_get_ms();
//check if a sensor update has occured
if ((sensor_update_serviced == FALSE) &&
(t%MAZE_LOGIC_PERIOD == 0) && (t != maze_logic_t_last))
{
sensor_update_serviced = TRUE;
// finishing condition is a grey read for a set period
if(is_grey(sensor_front_value) && front_crossed_grey == FALSE)
{
front_crossed_grey = TRUE;
grey_time_start = t; //TODO: adjust so that finishing condition is a 1/2 whole sweeps on grey line
}
else if (is_grey(sensor_front_value) && front_crossed_grey == TRUE)
{
//
if ((grey_time_start + GREY_TIME) <= t )
{
// Finish line found. Stop robot.
maze_completed(); // wait for button push
front_crossed_grey = FALSE;
}
}
else
{
front_crossed_grey = FALSE;
}
//see if the front sensor crosses the line in case we run into a gap
if(is_black(sensor_front_value)&&front_crossed_black == FALSE)
{
front_crossed_black = TRUE;
//check for false finish line
if(front_crossed_grey)
front_crossed_grey = FALSE; //false alarm
}
// when both rear sensors go black, this indicates an intersection (turns included).
// try turning left
if(is_black(sensor_left_value) && is_black(sensor_right_value))
{
sweep_ended = TRUE;
motor_set(0, 255);
PORTB |= BIT(3);
PORTB |= BIT(4);
}
//when both sensors are completely white this indicates a dead end or a tape-gap
else if (is_white(sensor_left_value) && is_white(sensor_right_value))
{
sweep_ended = TRUE;
PORTB &= ~BIT(3);
PORTB &= ~BIT(4);
//current_action = ON_WHITE;
//Check if the front sensor is on black, or has been during the last sweep.
if(is_black(sensor_front_value) | front_crossed_black)
motor_set(255, 255);
else if (is_white(sensor_front_value))
motor_set(-255, 255);
}
//sweep to the side that reads the darkest value
else if (sensor_left_value + SENSOR_TOLLERANCE < sensor_right_value)
{
PORTB &= ~BIT(3);
PORTB |= BIT(4);
if (current_action == SWEEP_LEFT)
sweep_ended = TRUE;
current_action = SWEEP_RIGHT;
motor_set(forward_speed + turn_speed, forward_speed);
}
else if(sensor_right_value + SENSOR_TOLLERANCE< sensor_left_value)
{
PORTB |= BIT(3);
PORTB &= ~BIT(4);
if (current_action == SWEEP_RIGHT)
sweep_ended = TRUE;
current_action = SWEEP_LEFT;
motor_set(forward_speed, forward_speed+ turn_speed);
}
//If a new sweep started this cycle, find how long it took
if (sweep_ended)
{
//reset front black crossing detection variable
sweep_ended = FALSE;
if (front_crossed_black)
front_crossed_black = FALSE;
//Calculate sweep time
sweep_del_t_last = t - sweep_end_t_last;
sweep_end_t_last = t;
writeCircBuf(&sweep_times, sweep_del_t_last);
//adjust turn_speed for battery level.
if (sweep_del_t_last > IDEAL_SWEEP_TIME)
{
turn_speed += 5;
}
if (sweep_del_t_last < IDEAL_SWEEP_TIME)
{
turn_speed -= 5;
}
turn_speed = regulate_within(turn_speed, MIN_TURN_SPEED, MAX_TURN_SPEED);
}
}
//Sensor value update
if((t%SAMPLE_PERIOD == 0) & (t!=sample_t_last))
{
sample_t_last = t;
//read in analog values
sensor_update(CHANNEL_SENSOR_LEFT, &left_buffer, &sensor_left_value );
sensor_update(CHANNEL_SENSOR_RIGHT, &right_buffer, &sensor_right_value );
sensor_update(CHANNEL_SENSOR_FRONT, &front_buffer, &sensor_front_value );
sensor_update_serviced = FALSE;
}
//display debug information
if((t%UART_PERIOD == 0) & (t != UART_t_last) & UART_ENABLED)
{
UART_t_last = t;
sprintf(buffer, "sweep_time: %u \n", sweep_del_t_last);
UART_Write(buffer);
sprintf(buffer, "L: %u F: %u R: %u", sensor_left_value, sensor_front_value, sensor_right_value);
UART_Write(buffer);
UART_Write("\n");
}
}
}
/*!
* Hauptprogramm des Bots. Diese Schleife kuemmert sich um seine Steuerung.
*/
int main (void){
#endif
#ifdef PC
/*!
* Hauptprogramm des Bots. Diese Schleife kuemmert sich um seine Steuerung.
*/
int main (int argc, char *argv[]){
int ch;
int start_server = 0; /*!< Wird auf 1 gesetzt, falls -s angegeben wurde */
char *hostname = NULL; /*!< Speichert den per -t uebergebenen Hostnamen zwischen */
// Die Kommandozeilenargumente komplett verarbeiten
while ((ch = getopt(argc, argv, "hst:")) != -1) {
switch (ch) {
case 's':
// Servermodus [-s] wird verlangt
start_server = 1;
break;
case 't':
// Hostname, auf dem ct-Sim laeuft wurde
// uebergeben. Der String wird in hostname
// gesichert.
{
const int len = strlen(optarg);
hostname = malloc(len + 1);
if (NULL == hostname)
exit(1);
strcpy(hostname, optarg);
}
break;
case 'h':
default:
// -h oder falscher Parameter, Usage anzeigen
usage();
}
}
argc -= optind;
argv += optind;
if (start_server != 0) // Soll der TCP-Server gestartet werden?
{
printf("ARGV[0]= %s\n",argv[1]);
tcp_server_init();
tcp_server_run();
} else {
printf("c't-Bot\n");
if (hostname)
// Hostname wurde per Kommandozeile uebergeben
tcp_hostname = hostname;
else {
// Der Zielhost wird per default durch das Macro IP definiert und
// tcp_hostname mit einer Kopie des Strings initialisiert.
tcp_hostname = malloc(strlen(IP) + 1);
if (NULL == tcp_hostname)
exit(1);
strcpy(tcp_hostname, IP);
}
}
#endif
#ifdef TEST_AVAILABLE_MOTOR
uint16 calls=0; /*!< Im Testfall zaehle die Durchlaeufe */
#endif
#ifdef LOG_AVAILABLE
printf("Logging is on (");
#ifdef LOG_UART_AVAILABLE
printf("UART");
#endif
#ifdef LOG_CTSIM_AVAILABLE
printf("CTSIM");
#endif
#ifdef LOG_DISPLAY_AVAILABLE
printf("DISPLAY");
#endif
#ifdef LOG_STDOUT_AVAILABLE
printf("STDOUT");
#endif
printf(")\n");
#else
printf("Logging is off!\n ");
#endif
init();
#ifdef WELCOME_AVAILABLE
display_cursor(1,1);
display_printf("c't-Roboter");
LED_set(0x00);
#ifdef LOG_AVAILABLE
LOG_DEBUG(("Hallo Welt!"));
#endif
#endif
#ifdef TEST_AVAILABLE_COUNTER
display_screen=2;
resets=eeprom_read_byte(&resetsEEPROM)+1;
eeprom_write_byte(&resetsEEPROM,resets);
/* Lege den Grund für jeden Reset im EEPROM ab */
eeprom_write_byte(&resetInfoEEPROM+resets,reset_flag);
#endif
/*! Hauptschleife des Bot */
for(;;){
#ifdef MCU
bot_sens_isr();
#endif
#ifdef TEST_AVAILABLE
show_sensors();
#endif
// Testprogramm, dass den Bot erst links, dann rechtsrum dreht
#ifdef TEST_AVAILABLE_MOTOR
calls++;
if (calls == 1)
motor_set(BOT_SPEED_SLOW,-BOT_SPEED_SLOW);
else if (calls == 501)
motor_set(-BOT_SPEED_SLOW,BOT_SPEED_SLOW);
else if (calls== 1001)
motor_set(BOT_SPEED_STOP,BOT_SPEED_STOP);
else
#endif
// hier drin steckt der Verhaltenscode
#ifdef BEHAVIOUR_AVAILABLE
if (sensors_initialized ==1 )
bot_behave();
else
printf("sensors not initialized\n");
#endif
#ifdef MCU
#ifdef BOT_2_PC_AVAILABLE
// static int16 lastTimeCom =0;
bot_2_pc_inform(); // Den PC ueber Sensorern und aktuatoren informieren
bot_2_pc_listen(); // Kommandos vom PC empfangen
// if (timer_get_s() != lastTimeCom) { // sollte genau 1x pro Sekunde zutreffen
// lastTimeCom = timer_get_s();
// }
#endif
#endif
#ifdef LOG_AVAILABLE
//LOG_DEBUG(("LOG TIME %d s", timer_get_s()));
#endif
// Alles Anzeigen
#ifdef DISPLAY_AVAILABLE
display();
#endif
#ifdef PC
wait_for_time(100000);
#endif
#ifdef MCU
// delay(10);
#endif
}
/*! Falls wir das je erreichen sollten ;-) */
return 1;
}
void USART2_IRQHandler()
{
if(USART2->SR & USART_SR_RXNE)
{
uint8_t data = USART2->DR;
switch(data)
{
case 0x01: // STARt
armed = 0xFF;
temp = 0;
break;
case 0x02: //STOP
armed = 0;
curr_angle.x = 0;
curr_angle.y = 0;
curr_angle.z = 0;
pid[0].last_error = 0;
pid[0].sum_error = 0;
pid[1].last_error = 0;
pid[1].sum_error = 0;
motor_set(1, 0);
motor_set(2, 0);
motor_set(3, 0);
motor_set(4, 0);
break;
case 0x03: //czytaj k¹t
usart_put(0xFF);
usart_put(0x27);
dec2hascii((int)(curr_angle.x*100), 8);
dec2hascii((int)(curr_angle.y*100), 8);
usart_put(0x0A);
break;
case 0x04:
dec2hascii(pilot.throttle_up_down, 8);
usart_put(0x0A);
break;
case 0x0A:
buffer_parse();
break;
default:
buffer[buffer_pos] = data;
buffer_pos++;
break;
}
}
}
void process_bytes(){
uint16_t nb_received = CDC_Device_BytesReceived(&VirtualSerial_CDC_Interface);
uint8_t c;
for( uint16_t i = 0; i < nb_received ; i++ ){
c = CDC_Device_ReceiveByte(&VirtualSerial_CDC_Interface);
switch (c){
case 'l':
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
for (uint8_t x = 0; x < 255; x++){
led_set(255,20,x);
Delay_MS(10);
}
led_clear();
println("LED test finished");
break;
#ifdef ONBOARD_STUFF
case 'm':{
switch (motor_get()){
case 0: // TODO use ranges instead of exact values
println("Motor ON (PWM @255)");
motor_set(255);
break;
case 255:
println("Motor ON (PWM @100)");
motor_set(100);
break;
case 100:
println("Motor OFF");
motor_set(0);
break;
}
}break;
case 'd':{
fprintf(&USBSerialStream, "GP2Y PM: %.1f ug/m3\r\n", gp2y_get_pm() );
}break;
case 'k':
fprintf(&USBSerialStream, "humidity: %.0f %%\r\n", cc2dxx_get_humidity());
Delay_MS(100);
fprintf(&USBSerialStream, "temp: %.2f deg C\r\n", cc2dxx_get_temperature());
break;
#endif
#ifdef SENSORBOARD
case 'i':
fprintf(&USBSerialStream, "TCOV: %d\r\n", iaqengine_get_ppm());
break;
#endif
#ifdef WIFI
case 'w':
wifi_print_MAC();
wifi_print_ip();
break;
case 'y':
wifi_config_set_dev();
wifi_connect_to_ap(wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode);
wifi_print_ip();
wifi_ping(192,168,1,1);
break;
case 's':
wifi_resolve_backend_ip();
break;
/*case 'e':
fprintf(&USBSerialStream, "is set? %u\r\n", wifi_config_is_set());
fprintf(&USBSerialStream, "set dev %u\r\n", wifi_config_set_dev());
fprintf(&USBSerialStream, "is set? %u\r\n", wifi_config_is_set());
fprintf(&USBSerialStream, "save %u\r\n", wifi_config_save());
fprintf(&USBSerialStream, "%s %s %u\r\n", wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode);
fprintf(&USBSerialStream, "set lol %u\r\n", wifi_config_set("lol", "lolilol", 2));
fprintf(&USBSerialStream, "%s %s %u\r\n", wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode);
fprintf(&USBSerialStream, "load %u\r\n", wifi_config_load());
fprintf(&USBSerialStream, "%s %s %u\r\n", wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode);
wifi_config_mem_clear();
fprintf(&USBSerialStream, "clear then load %u\r\n", wifi_config_load());
break;*/
case 'p':{
println("Trying to POST");
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
wifi_config_set_dev();
/* // There is a problem with that, it hangs sometimes ><
if (!wifi_config_is_set()){
if (!wifi_config_load()){
#ifdef DEV
printf("setting up DEV config");
wifi_config_set_dev();
#else
printf("No config anywhere!");
return;
// TODO what do we do if it's not configured?
#endif
}
}*/
fprintf(&USBSerialStream, "%s %s %u\r\n", wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode);
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
// TODO check if the SSID is available before trying to connect or it might hang... :/
if( !wifi_connect_to_ap(wifi_ap_ssid, wifi_ap_password, wifi_ap_encrypt_mode) == WERR_OK){
println("AP NOK");
} else {
println("AP OK");
wifi_print_ip();
uint32_t backend_ip;
uint16_t backend_port = 80;
//backend_ip = cc3000_IP2U32(54,221,218,154);
//backend_ip = cc3000_IP2U32(67,215,65,132);
//backend_ip = cc3000_IP2U32(192,168,1,11);
//backend_ip = cc3000_IP2U32(192,168,42,102);
backend_ip = wifi_resolve_backend_ip();
print_ip_helper("backend", backend_ip);
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
if (cc3000_ping(backend_ip, 3, 500, 32)){
fprintf(&USBSerialStream, "ping OK\r\n");
} else {
fprintf(&USBSerialStream, "ping timeout\r\n");
}
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
void* sock = cc3000_connectTCP(backend_ip, backend_port);
if (cc3000_cli_connected(sock)){
println("cli connected");
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
uint16_t res = 0;
res += cc3000_cli_fastrprint(sock, "POST /v1/airboxlab/ HTTP/1.1\n");
res += cc3000_cli_fastrprint(sock, "User-Agent: abl1.0\n");
res += cc3000_cli_fastrprint(sock, "Host: api.airboxlab.com\n");
res += cc3000_cli_fastrprint(sock, "Accept: */*\n");
//res += cc3000_cli_fastrprint(sock, "Content-Type: application/json\n");
res += cc3000_cli_fastrprint(sock, "Content-Length: 35\n\n");
res += cc3000_cli_fastrprint(sock, "5040f3bd20c8,20.7,53.9,1255,11.1,78\n");
fprintf(&USBSerialStream, "written: %u / %u\r\n", res, 171);
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
Delay_MS(1000);
uint8_t nb_available = cc3000_cli_available(sock);
fprintf(&USBSerialStream, "Returned %u\r\n", nb_available);
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
for (int i = 0; i<cc3000_cli_available(sock); i++){
fprintf(&USBSerialStream, "%c" ,cc3000_cli_read(sock));
}
println("");
cc3000_cli_close(sock);
println("closed");
} else {
println("cli not connected");
}
}
}break;/*
case 'W':{ // setup wifi
int nb_returned;
println("SSID?");
println("password?");
println("encryption mode number?[0:none, 1:WEP, 2:WPA, 3:WPA2]");
uint16_t _mode;
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
while(1){ // TODO timeout? opt-out?
_mode = fgetc(&USBSerialStream);
printf("read: %i", _mode);
if(_mode <= 3){
wifi_ap_encrypt_mode = _mode;
fprintf(&USBSerialStream, "Mode = %u\r\n", _mode);
break;
} else {
fprintf(&USBSerialStream, "Not a valid mode. Please choose number between [0:none, 1:WEP, 2:WPA, 3:WPA2]\r\n");
CDC_Device_USBTask(&VirtualSerial_CDC_Interface);
USB_USBTask();
}
}
}break;*/
#endif
default:
println("Airboxlab serial mode: press 'l' key to test LEDs");
break;
}
}
}
int main (int argc, char *argv[]) {
spawn_term((argc >= 2) ? argv[1] : NULL);
char cmd[51]; // stores the command that was entered
char* token[MAX_TOKENS]; // points to parts of the command
unsigned i;
int pwm, motor_num, target_setting;
for (;;) {
printf (LINE_START);
cmd_ok = 0;
assert(fgets (cmd, 50, stdin));
// use strtok to grab each part of the command
token[0] = strtok (cmd, " \n");
for (i = 1; i < MAX_TOKENS; i++)
token[i] = strtok (NULL, " \n");
if (token[0] == NULL)
help ();
else if (!strcmp(token[0], "q") || !strcmp(token[0], "exit"))
exit_safe();
else if (!strcmp(token[0], ":q"))
printf ("this isnt vim lol\n");
else if (!strcmp(token[0], "help")) {
if (token[1] == NULL)
help ();
else if (!strcmp(token[1],"motor") || !strcmp(token[1],"m"))
help_motor();
else if (!strcmp(token[1],"dyn") || !strcmp(token[1],"d"))
help_dyn();
else if (!strcmp(token[1],"power") || !strcmp(token[1],"p"))
help_power();
}
else if (!strcmp(token[0], "motor") || !strcmp(token[0], "m")) {
if (token[1] == NULL || !strcmp (token[1], "status"))
motor_status();
else if (!strcmp (token[1], "all")) {
pwm = atoi_safe(token[2]); // note an invalid token will cause pwm = 0, which is ok
motor_set (pwm, H_ALL);
}
else if (!strcmp (token[1], "fwd")) {
pwm = atoi_safe(token[2]); // note an invalid token will cause pwm = 0, which is ok
motor_set (pwm, H_FWD);
}
else if (!strcmp (token[1], "rise")) {
pwm = atoi_safe(token[2]); // note an invalid token will cause pwm = 0, which is ok
motor_set (pwm, H_RISE);
}
else {
motor_num = atoi_safe (token[1]); // 1-indexed
motor_num--; // Internally, this is 0-indexed
pwm = atoi_safe (token[2]);
switch (motor_num) {
case M_FRONT_LEFT: motor_set (pwm, H_FRONT_LEFT); break;
case M_FRONT_RIGHT: motor_set (pwm, H_FRONT_RIGHT); break;
case M_FWD_LEFT: motor_set (pwm, H_FWD_LEFT); break;
case M_FWD_RIGHT: motor_set (pwm, H_FWD_RIGHT); break;
case M_REAR: motor_set (pwm, H_REAR); break;
default: printf ("**** Invalid motor number.\n");
}
}
}
else if (!strcmp(token[0], "power") || !strcmp(token[0], "p")) {
if (token[1] == NULL || !strcmp (token[1], "status"))
power_status();
else if (!strcmp (token[1], "on") || !strcmp (token[1], "1")) {
power_on();
}
else if (!strcmp (token[1], "start") || !strcmp (token[1], "2")) {
startup_sequence();
}
else if (!strcmp (token[1], "off") || !strcmp (token[1], "0")) {
power_off();
}
}
else if (!strcmp(token[0], "dyn") || !strcmp(token[0], "d")) {
if (token[1] == NULL)
dyn_status();
else if (!strcmp (token[1], "depth")) {
target_setting = atoi_safe(token[2]); // Note an invalid token will cause target_setting = 0, which is ok
dyn_set_target_depth(target_setting);
}
}
if (cmd_ok != 1)
cmd_error();
}
}