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
}
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
}
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();
}
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);
}
}
/*
* 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;
}
}
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;
}
int readArgument(int defaultValue = 255){
char *arg;
arg = sCmd.next();
if (arg != NULL) {
return atoi(arg);
}
else{
return defaultValue;
}
}
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);
}
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();
}
/*
* 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();
}
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);
}
/*
* 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
}
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);
}
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);
}
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);
}
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
}
void loop() {
sCmd.readSerial();
}
