Exemple #1
0
uint8_t Gimbal::SBGC_cmd_angles_unpack(angulos &p, SerialCommand &cmd) {

	for (uint8_t i = 0; i < 3; i++) {

		switch (i)
		{
		case 0:
			p.roll = cmd.readWord();
			p.roll = p.roll*ESCALA;
			break;
		case 1:
			p.pitch = cmd.readWord();
			p.pitch = p.pitch*ESCALA;
			break;
		case 2:
			p.yaw = cmd.readWord();
			p.yaw = p.yaw*ESCALA;
			break;
		}

		//Leemos dos veces, para consumir los datos rc_angle y rc_speed que no son necesarios.
		cmd.readWord();
		cmd.readWord();
	}

	if (cmd.checkLimit()) return 0;
	else return PARSER_ERROR_WRONG_DATA_SIZE;
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_trigger_pack(SBGC_cmd_trigger_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_TRIGGER_PIN);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeByte(p.pin);
		cmd.writeByte(p.state);
	#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_api_virt_ch_control_pack(SBGC_cmd_api_virt_ch_control_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_API_VIRT_CH_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		for(uint8_t i=0; i<SBGC_API_VIRT_NUM_CHANNELS; i++) {
			cmd.writeByte(p.data[i]);
		}
	#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_servo_out_pack(SBGC_cmd_servo_out_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_SERVO_OUT);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		for(uint8_t i=0; i<8; i++) {
			cmd.writeWord(p.servo[i]);
		}
	#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_set_adj_vars_pack(SBGC_cmd_set_adj_vars_var_t vars[], uint8_t vars_num, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_SET_ADJ_VARS_VAL);
	cmd.writeByte(vars_num); // number of variables
	
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		cmd.writeBuf(vars, sizeof(SBGC_cmd_set_adj_vars_var_t)*vars_num);
	#else
		for(uint8_t i=0; i<vars_num; i++) {
			cmd.writeByte(vars[i].id);
			cmd.writeLong(vars[i].val);
		}
	#endif
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_ext_pack(SBGC_cmd_control_ext_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeBuf(p.mode, 3);
		for(uint8_t i=0; i<3; i++) {
			cmd.writeWord(p.data[i].speed);
			cmd.writeWord(p.data[i].angle);
		}
	#endif
}
Exemple #7
0
void CommandSet::go()
{
    /* holonomics and shit */
    float x_vel = atof(sCmd.next());
    float y_vel = atof(sCmd.next());
    float r_vel = atof(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.println(F("Going"));
    #endif

    _go(x_vel, y_vel, r_vel);
}
/* Packs command structure to SerialCommand object */
void SBGC_cmd_control_pack(SBGC_cmd_control_t &p, SerialCommand &cmd) {
	cmd.init(SBGC_CMD_CONTROL);
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		memcpy(cmd.data, &p, sizeof(p));
		cmd.len = sizeof(p);
	#else
		cmd.writeByte(p.mode);
		cmd.writeWord(p.speedROLL);
		cmd.writeWord(p.angleROLL);
		cmd.writeWord(p.speedPITCH);
		cmd.writeWord(p.anglePITCH);
		cmd.writeWord(p.speedYAW);
		cmd.writeWord(p.angleYAW);
	#endif
}
Exemple #9
0
void CommandSet::rotate()
{
    const int motor_power   = atoi(sCmd.next());
    const long delta        = atoi(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("rotating at "));
    Serial.print(motor_power);
    Serial.print(F(" to "));
    Serial.print(delta);
    Serial.println(F(" stops"));
    #endif

    for (size_t i=0; i < motor_count; i++){
        state.motors[i]->power = motor_power;
        state.initial_displacement[i] = state.motors[i]->disp;
    }

    state.rotation_delta = delta;

    processes.enable((state.rotation_process)->id);

    write_powers();
}
Exemple #10
0
void CommandSet::grab()
{
    const int direction = atoi(sCmd.next());

    if (state.grabber_state != Open && state.grabber_state != Closed) {
        Serial.println(F("N - grab"));
        return;
    }

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("grabbing "));
    Serial.println(direction);
    #endif

    const pid_t pid         = state.grab_handler->id;
    const int motor_power   = 200;

    /* If we're open, doing that again will bugger the vision plate */
    if (direction) {
        state.grabber_state = Closing;
        motorForward(grabber_port, motor_power);
        processes.change(pid, 300L);
    } else if (state.grabber_state != Open) {
        state.grabber_state = Opening;
        motorBackward(grabber_port, motor_power);
        processes.change(pid, 280L);
    } else {
        return;
    }

    processes.enable(pid);
    processes.forward(pid);
}
void executeSerialCommands()
{
	while (Serial.available()) {
		char c = Serial.read();
		Serial.print(c); // echo
		commandProcessor.update(c);
	}
}
Exemple #12
0
/*
* Unpacks SerialCommand object to vars_buf[var_num].
* 'var_num' specifies the buffer capacity.
* On return, 'var_num' will be set to actual number of received variables.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_set_adj_vars_unpack(SBGC_cmd_set_adj_vars_var_t vars_buf[], uint8_t &vars_num, SerialCommand &cmd) {
	uint8_t num = cmd.readByte(); // actual number of variables
	if(num <= vars_num) {
		vars_num = num;
		#ifdef SBGC_CMD_STRUCT_ALIGNED
				cmd.readBuf(vars_buf, sizeof(SBGC_cmd_set_adj_vars_var_t)*vars_num);
		#else
			for(uint8_t i=0; i<num; i++) {
				vars_buf[i].id = cmd.readByte();
				vars_buf[i].val = cmd.readLong();
			}
		#endif
		
		if(cmd.checkLimit()) return 0;
		else return PARSER_ERROR_WRONG_DATA_SIZE;
	} else {
		return PARSER_ERROR_BUFFER_IS_FULL;
	}
}
Exemple #13
0
void CommandSet::pixels()
{
    byte red   = (byte) atoi(sCmd.next());
    byte green = (byte) atoi(sCmd.next());
    byte blue  = (byte) atoi(sCmd.next());

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.print(F("Set pixel colour to "));
    Serial.print(red,HEX);
    Serial.print(green,HEX);
    Serial.println(blue,HEX);
    #endif

    for (uint16_t i = 0; i < 10; i++) {
        state.strip.setPixelColor(i, red, green, blue);
    }
    state.strip.show();
}
void setup()
{
    // initialize the serial communication:
    serial.init(TTYACM0, 115200, openComport);
    serial.setPacketHandler(handlePacket);
    sCmd.addCommand(1, respondData);
    gettimeofday(&start, NULL);
    gettimeofday(&startLoop, NULL);
    uiRxPacketCtr=0;
    uiTxPacketCtr=0;
}
Exemple #15
0
int readArgument(int defaultValue = 255){
  char *arg;

  arg = sCmd.next();
  if (arg != NULL) {
    return atoi(arg);
  }
  else{
    return defaultValue;
  }
}
Exemple #16
0
void CommandSet::proc_toggle()
{
    pid_t pid = (pid_t) atoi(sCmd.next());

    if (pid == NULL) {
        Serial.println("N - ptog");
        return;
    }
    
    process* proc = processes.get_by_id(pid);

    if (proc == NULL) {
        Serial.print(F("Unknown pid "));
        Serial.println(pid);
        return;
    }
    
    proc->enabled ? processes.disable(pid) : processes.enable(pid);

    Serial.print(F("toggled pid "));
    Serial.println(pid);
}
Exemple #17
0
void CommandSet::move()
{
    int new_powers[motor_count];

    for (int i=0; i < motor_count; i++){
        new_powers[i] = atoi(sCmd.next());
        /* Apply direction correction here */
        new_powers[i] *= state.motors[i]->direction;
    }

    Serial.println(F("A"));
    #ifdef FW_DEBUG
    Serial.println(F("Moving"));
    #endif
        
    /* update speeds of all drive motors */
    for(int i=0; i < motor_count; i++){
        state.motors[i]->power = state.motors[i]->desired_power = new_powers[i];
        state.motors[i]->disp_delta = 0;
    }
    write_powers();
}
Exemple #18
0
/*
 * This method basically just allows us to execute the go command internally.
 */
void CommandSet::_go(float x_vel, float y_vel, float r_vel)
{
    int power   = atoi(sCmd.next());
    power = power > 0 && power <= 255 ? power : 255;

    float new_powers[motor_count];

    new_powers[0] =
        state.velo_coupling_mat[0][0] * x_vel +
        state.velo_coupling_mat[0][1] * y_vel +
        state.velo_coupling_mat[0][2] * r_vel;

    new_powers[1] =
        state.velo_coupling_mat[1][0] * x_vel +
        state.velo_coupling_mat[1][1] * y_vel +
        state.velo_coupling_mat[1][2] * r_vel;

    new_powers[2] =
        state.velo_coupling_mat[2][0] * x_vel +
        state.velo_coupling_mat[2][1] * y_vel +
        state.velo_coupling_mat[2][2] * r_vel;

    float largest = 0.0;
    for (int i=0; i<3; i++){
        if (largest < fabs(new_powers[i]))
            largest = fabs(new_powers[i]);
    }

    for (int i=0; i<3; i++){
        new_powers[i] = round(new_powers[i] * (1.0/largest) * (float) power);
    }

    for(int i=0; i < motor_count; i++){
        state.motors[i]->power = state.motors[i]->desired_power = (int) new_powers[i];
        state.motors[i]->disp_delta = 0;
    }
    write_powers();
}
Exemple #19
0
void setup() {
  Serial.begin(115200);
  pinMode(DRIVE_PIN1, OUTPUT);
  pinMode(DRIVE_PIN2, OUTPUT);
  pinMode(DRIVE_ENABLE_PIN, OUTPUT);

  myservo.attach(STEER_SERVO);
  myservo.write(DEFAULT_STEER);

  Serial.println("Ready");

  sCmd.addCommand("T", throttle);
  sCmd.addCommand("S", steer);
  sCmd.addCommand("R", reset);
  sCmd.addCommand("@", ping);
  sCmd.addCommand("V", voltage);
  sCmd.addCommand("D", distance);
  sCmd.addDefaultHandler(unrecognized);
}
Exemple #20
0
/*
* Unpacks SerialCommand object to command structure.
* Returns 0 on success, PARSER_ERROR_XX code on fail.
*/
uint8_t SBGC_cmd_realtime_data_unpack(SBGC_cmd_realtime_data_t &p, SerialCommand &cmd) {
	#ifdef SBGC_CMD_STRUCT_ALIGNED
		if(cmd.len <= sizeof(p)) {
			memcpy(&p, cmd.data, cmd.len);
			return 0;
		} else {
			return PARSER_ERROR_WRONG_DATA_SIZE;
		}
	#else
		for(uint8_t i=0; i<3; i++) {
			p.sensor_data[i].acc_data = cmd.readWord();
			p.sensor_data[i].gyro_data = cmd.readWord();
		}
		p.serial_error_cnt = cmd.readWord();
		p.system_error = cmd.readWord();
		cmd.skipBytes(4); // reserved
		cmd.readWordArr(p.rc_raw_data, SBGC_RC_NUM_CHANNELS);
		cmd.readWordArr(p.imu_angle, 3);
		cmd.readWordArr(p.frame_imu_angle, 3);
		cmd.readWordArr(p.target_angle, 3);
		p.cycle_time_us = cmd.readWord();
		p.i2c_error_count = cmd.readWord();
		cmd.readByte(); // reserved
		p.battery_voltage = cmd.readWord();
		p.state_flags1 = cmd.readByte();
		p.cur_imu = cmd.readByte();
		p.cur_profile = cmd.readByte();
		cmd.readBuf(p.motor_power, 3);
		
		if(cmd.id == SBGC_CMD_REALTIME_DATA_4) {
			cmd.readWordArr(p.rotor_angle, 3);
			cmd.readByte(); // reserved
			cmd.readWordArr(p.balance_error, 3);
			p.current = cmd.readWord();
			cmd.readWordArr(p.magnetometer_data, 3);
			p.imu_temp_celcius = cmd.readByte();
			p.frame_imu_temp_celcius = cmd.readByte();
			cmd.skipBytes(38);
		}
		
		
		if(cmd.checkLimit()) return 0;
		else return PARSER_ERROR_WRONG_DATA_SIZE;
	#endif
}
Exemple #21
0
void CommandSet::readSerial()
{
    // Poll serial buffer & check against command set
    sCmd.readSerial();
}
void handleSerial()
{
  sCmd.readSerial();
}
void handlePacket(byte* packet, byte length){
    sCmd.readSerial(packet, length);
}
Exemple #24
0
int main(void)
{
	bool cmd_ok = false;

	wdt_enable(WDTO_8S);

	// ADC enable; prescaler: 128 (156 KHz @ 20MHz)
	ADCSRA |= (_BV(ADEN) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0));
	DDRC |= _BV(DDC0);  // LED:OUTPUT

	// NLOCK: locked
	DDRC |= _BV(DDC2);  // OUTPUT
	PORTC &= ~_BV(PORTC2);  // LOW

#ifdef DEBUG
	int m = DEBUG_CMD_DELAY;  // ms
	fdevopen(&serial_putc, 0);  // open the stdout and stderr streams
#endif

	sys_serial.begin(57600);
	IF_DEBUG(printf_P(PSTR("BOOT\r\n")));

	// initiate a sync with the daemon after boot
	// there is a 1:65536 chance that sync will not be lost
	// the daemon will initialize the device by the command
	cmd.set(CMD_COMMON, COMMON_RESET, 0, 0);
	common_device.confirm_boot(&cmd);
	//cmd.reset();  // look to the SerialCommand::read

	sys_log.begin();
	SPI.begin();
	if (radio.begin()) {
		 network.begin(RF24_CHANEL, rx_node);
		 nrf_device.setup(&network);
	}

	while(true)
	{
		wdt_reset();

		// CMD_NRF_FORWARD
		if (nrf_device.read(&cmd)) {
			if (!nrf_device.run(&cmd)) cmd.send_header(-1);
			//cmd.reset();
		}

		// CMD_RNG_SEND
		for (int i = 0; i < WDT_BITS_PER_CYCLE; i++) {
			if (rng_device.read(&cmd)) {
				if (!rng_device.run(&cmd)) cmd.send_header(-1);
				//cmd.reset();
			}
		}

		// watchdog trigger
		wdt_device.update();

#ifdef DEBUG
		if (--m <= 0) {
			m = DEBUG_CMD_DELAY;
			//cmd.set(CMD_COMMON, 1, 1460792071, 0); // look to the SerialCommand::read
			//sys_log.write(sys_time.now(), LOG_BOOT);
		}
		//_delay_ms(1);
#endif

		// serial commands
		if (cmd.read()) {
#ifdef DEBUG
			blink_once();
#endif
			cmd_ok = false;
			switch (cmd.get_type()) {
				case CMD_COMMON:
					cmd_ok = common_device.run(&cmd);
					break;
				case CMD_WDT:
					cmd_ok = wdt_device.run(&cmd);
					break;
				case CMD_RNG:
					cmd_ok = rng_device.run(&cmd);
					break;
				case CMD_NRF:
					cmd_ok = nrf_device.run(&cmd);
					break;
				default:
					break;
			}
			if (!cmd_ok) cmd.send_header(-1);
			//cmd.reset();
		}
	} // while(true)

	sys_serial.end();
	return 0;
} // main
void initCommandProcessor()
{
	commandProcessor.setDefaultHandler(commandDefault);
}
Exemple #26
0
void CommandSet::help()
{
    Serial.println(F("Valid input commands: (some have arguments)"));
    delay(10);
    sCmd.dumpCommandSet();
}
void addCommand(const char *cmd, void(*function)(), const char *wildcard)
{
  sCmd.addCommand(cmd, function);
}
Exemple #28
0
void CommandSet::setup()
{
    /* Setup callbacks for SerialCommand commands */
    sCmd.addCommand("L",       this->led);          // Toggles LED
    sCmd.addCommand("ping",    this->ping);         // Check serial link
    sCmd.addCommand("help",    this->help);

    /* Movement commands */
    sCmd.addCommand("M",       this->move);         // Runs wheel motors
    sCmd.addCommand("S",       this->stop);         // Force stops all motors
    sCmd.addCommand("G",       this->go);           // Runs wheel motors with correction

    /* Read from rotary encoders */
    sCmd.addCommand("speeds",  this->speeds);

    /* Misc commands */
    sCmd.addCommand("pixels",  this->pixels);       // Set LED colour

    sCmd.addCommand("grab",    this->grab);         // Grab 0 or 1
    sCmd.addCommand("kick",    this->kick);         // Kick
    sCmd.addCommand("shuntkick", this->shuntkick);  // Shuntkick

    sCmd.addCommand("rotate",  this->rotate);       // Rotate

    /* Debug and inspection commands */

    sCmd.addCommand("ps",      this->proc_dump);    // Show process status
    sCmd.addCommand("ptog",    this->proc_toggle);  // Enable or disable by pid on the fly
    
    sCmd.setDefaultHandler(this->unrecognized);   // Handler for command that isn't matched
}
Exemple #29
0
void loop() {
  sCmd.readSerial();
}