erratic.cc

机器人仿真软件

	this->motor_packet = new ErraticMotorPacket(param_idx);
	this->motor_packet->x_offset = 0;
	this->motor_packet->y_offset = 0;
	this->motor_packet->angle_offset = 0;

	// If requested, set max accel/decel limits
	{
		ErraticPacket *accel_packet;
		unsigned char accel_command[4];
		if(this->motor_max_trans_accel != 0) {
			accel_packet = new ErraticPacket();
			accel_command[0] = (command_e)set_max_trans_acc;
			accel_command[1] = (argtype_e)argint;
			accel_command[2] = this->motor_max_trans_accel & 0x00FF;
			accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8;
			accel_packet->Build(accel_command, 4);
			this->Send(accel_packet);
		}
		if(this->motor_max_trans_decel != 0) {
			accel_packet = new ErraticPacket();
			accel_command[0] = (command_e)set_max_trans_acc;
			accel_command[1] = (argtype_e)argnint;
			accel_command[2] = abs(this->motor_max_trans_decel) & 0x00FF;
			accel_command[3] = (abs(this->motor_max_trans_decel) & 0xFF00) >> 8;
			accel_packet->Build(accel_command, 4);
			this->Send(accel_packet);
		} else if(this->motor_max_trans_accel != 0) {
				accel_packet = new ErraticPacket();
				accel_command[0] = (command_e)set_max_trans_acc;
				accel_command[1] = (argtype_e)argnint;
				accel_command[2] = this->motor_max_trans_accel & 0x00FF;
				accel_command[3] = (this->motor_max_trans_accel & 0xFF00) >> 8;
				accel_packet->Build(accel_command, 4);
				this->Send(accel_packet);
		}
		
		if(this->motor_max_rot_accel != 0) {
			accel_packet = new ErraticPacket();
			accel_command[0] = (command_e)set_max_rot_acc;
			accel_command[1] = (argtype_e)argint;
			accel_command[2] = this->motor_max_rot_accel & 0x00FF;
			accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8;
			accel_packet->Build(accel_command, 4);
			this->Send(accel_packet);
		}
		if(this->motor_max_rot_decel != 0) {
			accel_packet = new ErraticPacket();
			accel_command[0] = (command_e)set_max_rot_acc;
			accel_command[1] = (argtype_e)argnint;
			accel_command[2] = abs(this->motor_max_rot_decel) & 0x00FF;
			accel_command[3] = (abs(this->motor_max_rot_decel) & 0xFF00) >> 8;
			accel_packet->Build(accel_command, 4);
			this->Send(accel_packet);
		} else if(this->motor_max_rot_accel != 0) {
				accel_packet = new ErraticPacket();
				accel_command[0] = (command_e)set_max_rot_acc;
				accel_command[1] = (argtype_e)argnint;
				accel_command[2] = this->motor_max_rot_accel & 0x00FF;
				accel_command[3] = (this->motor_max_rot_accel & 0xFF00) >> 8;
				accel_packet->Build(accel_command, 4);
				this->Send(accel_packet);
		}
	}
	
	{
		ErraticPacket *packet;
		unsigned char payload[4];

		// If requested, download custom PID settings
		if(this->pid_trans_p >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_trans_p;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_trans_p & 0x00FF;
			payload[3] = (this->pid_trans_p & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if(this->pid_trans_v >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_trans_v;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_trans_v & 0x00FF;
			payload[3] = (this->pid_trans_v & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if(this->pid_trans_i >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_trans_i;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_trans_i & 0x00FF;
			payload[3] = (this->pid_trans_i & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if(this->pid_rot_p >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_rot_p;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_rot_p & 0x00FF;
			payload[3] = (this->pid_rot_p & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if(this->pid_rot_v >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_rot_v;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_rot_v & 0x00FF;
			payload[3] = (this->pid_rot_v & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if(this->pid_rot_i >= 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pid_rot_i;
			payload[1] = (argtype_e)argint;
			payload[2] = this->pid_rot_i & 0x00FF;
			payload[3] = (this->pid_rot_i & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}

		// If requested, set PWM parameters
		if (this->motor_pwm_frequency > 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pwm_freq;
			payload[1] = (argtype_e)argint;
			payload[2] = this->motor_pwm_frequency & 0x00FF;
			payload[3] = (this->motor_pwm_frequency & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}
		if (this->motor_pwm_max_on > 0) {
			packet = new ErraticPacket();
			payload[0] = (command_e)set_pwm_max_on;
			payload[1] = (argtype_e)argint;
			payload[2] = this->motor_pwm_max_on & 0x00FF;
			payload[3] = (this->motor_pwm_max_on & 0xFF00) >> 8;
			packet->Build(payload, 4);
			this->Send(packet);
		}

		// If requested, make robot save all settings
		if (save_settings_in_robot) {
			packet = new ErraticPacket();
			payload[0] = (command_e)save_config;
			payload[1] = 0;
			payload[2] = 0;
			payload[3] = 0;
			packet->Build(payload, 4);
			this->Send(packet);
		}
	}
	
	// TODO: figure out what the right behavior here is

	// zero position command buffer
		//player_position_cmd_t zero;
		//memset(&zero,0,sizeof(player_position_cmd_t));
		//this->PutCommand(this->position_id,(void*)&zero,
			//sizeof(player_position_cmd_t),NULL);


	/* now spawn reading thread */
	((Erratic*)this)->StartThreads();
	return(0);
}

// Called by player when the driver is supposed to disconnect
int Erratic::Shutdown() {
	// We don't care, we'll never disconnect
	return 0;
}

// Is theoretically able to disconnect, but we don't do that
int Erratic::Disconnect() {
	printf("Shutting Erratic driver down\n");
	
	this->StopThreads();
	
	// If we're connected, send some kill commands before killing connections
	if (this->write_fd > -1) {
		unsigned char command[20],buffer[20];
		ErraticPacket packet;

		memset(buffer,0,20);
	
		command[0] = (command_e)stop;
		packet.Build(command, 1);
		packet.Send(this->write_fd);
		usleep(ROBOT_CYCLETIME);

		command[0] = (command_e)close_controller;
		packet.Build(command, 1);
		packet.Send(this->write_fd);
		usleep(ROBOT_CYCLETIME);
	
		close(this->write_fd);
		this->write_fd = -1;
	}
	if (this->read_fd > -1) {
		close(this->read_fd);
		this->read_fd = -1;
	}

	if (this->motor_packet) {
		delete this->motor_packet;
		this->motor_packet = NULL;
	}
	
	printf("Erratic has been shutdown");

	return(0);
}

// These call the supplied Driver::StartThread() method, but adds additional threads
void Erratic::StartThreads() {
	StartThread();
	pthread_create(&send_thread, 0, &SendThreadDummy, this);
	pthread_create(&receive_thread, 0, &ReceiveThreadDummy, this);
}
void Erratic::StopThreads() {
	pthread_cancel(send_thread);
	pthread_cancel(receive_thread);
	//TODO destroy threads?
	//TODO tickle threads to cancel point
	StopThread();
	
	// Cleanup mutexes
	pthread_mutex_trylock(&send_queue_mutex);
	pthread_mutex_unlock(&send_queue_mutex);
	
	pthread_mutex_trylock(&motor_packet_mutex);
	pthread_mutex_unlock(&motor_packet_mutex);
}

// Subscription is overridden to add a subscription count of our own
int Erratic::Subscribe(player_devaddr_t id) {
	int setupResult;                     
                                       
	// do the subscription               
	if((setupResult = Driver::Subscribe(id)) == 0)
	{
		// also increment the appropriate subscription counters

		if(Device::MatchDeviceAddress(id, this->position_id))
			this->position_subscriptions++;
		
		if(Device::MatchDeviceAddress(id, this->aio_id))
			this->aio_ir_subscriptions++;

		if(Device::MatchDeviceAddress(id, this->ir_id))
			this->aio_ir_subscriptions++;
	}                                    
                                       
	return(setupResult);                 
}                                      
int Erratic::Unsubscribe(player_devaddr_t id) {
	int shutdownResult;

	// do the unsubscription
	if((shutdownResult = Driver::Unsubscribe(id)) == 0)
	{
		// also decrement the appropriate subscription counter
		if(Device::MatchDeviceAddress(id, this->position_id))
		{
			this->position_subscriptions--;
			assert(this->position_subscriptions >= 0);
		}

		if(Device::MatchDeviceAddress(id, this->aio_id))
			this->aio_ir_subscriptions--;

		if(Device::MatchDeviceAddress(id, this->ir_id))
			this->aio_ir_subscriptions--;
	}

	return(shutdownResult);
}




/** Talking to the robot **/

// Listens to the robot
void Erratic::ReceiveThread() {
	for (;;) {
		pthread_testcancel();

		// Receive next packet from robot
		ErraticPacket packet;
		uint32_t waited = 0;
		uint8_t error_code;
		while ((error_code = packet.Receive(this->read_fd, 5000))) {
			waited += 5;
			printf("Lost serial communication with Erratic (%d) - no data received for %i seconds\n", error_code, waited);
		}
		
		if (waited)
			printf("Connection re-established\n");
		
		
		if (print_all_packets) {
			printf("Got: ");
			packet.PrintHex();
		}
		
		// Process the packet
		//Lock();
		
		switch(packet.packet[3]) {
			case (reply_e)motor:
			case (reply_e)motor+2:
			case (reply_e)motor+3:
				if (motor_packet->Parse(&packet.packet[3], packet.size-3)) {
					motor_packet->Fill(&erratic_data);
					PublishPosition2D();
					PublishPower();
				}
				break;
			case (reply_e)config:
				
				break;
			case (reply_e)ain:
				// This data goes in two places, analog input and ir rangers
				erratic_data.aio.voltages_count = packet.packet[4];
				erratic_data.ir.voltages_count = RobotParams[this->param_idx]->NumIR;
				erratic_data.ir.ranges_count = 2;
				for (unsigned int i_voltage = 0; i_voltage < erratic_data.aio.voltages_count ;i_voltage++) {
					erratic_data.aio.voltages[i_voltage] = (packet.packet[5+i_voltage*2]
						+ 256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
					erratic_data.ir.voltages[i_voltage] = (packet.packet[5+i_voltage*2]
						+ 256*packet.packet[6+i_voltage*2]) * (1.0 / 1024.0) * CPU_VOLTAGE;
					erratic_data.ir.ranges[i_voltage] = IRRangeFromVoltage(erratic_data.ir.voltages[i_voltage]);
				}
				PublishAIn();
				PublishIR();
				break;
			case (reply_e)debug:
				if (debug_mode) {
					printf("Debug message: ");
					for (uint8_t i = 3; i < packet.size-2 ;i++)
						printf("%c", packet.packet[i]);
					printf("\n");
				}
				break;
			default:
				if (debug_mode) {
					printf("Unrecognized packet: ");
					packet.Print();
				}
		}
		
		//Unlock();
	}
}
void* Erratic::ReceiveThreadDummy(void *driver) {
	((Erratic*)driver)->ReceiveThread();
	return 0;
}

// Sends to the robot
void Erratic::SendThread() {
	for (;;) {
		pthread_testcancel();
		
		// Get rights to the queue
		pthread_mutex_lock(&send_queue_mutex);
		
		// If there is nothing, we wait for signal
		if (!send_queue.size())
			pthread_cond_wait(&send_queue_cond, &send_queue_mutex);

		// Get first element and give the queue back
		ErraticPacket *packet = 0;
		if (send_queue.size()) {
			packet = send_queue.front();
			send_queue.pop();
		}
		pthread_mutex_unlock(&send_queue_mutex);
		
		// Send the packet and destroy it
		if (packet) {
			if (print_all_packets) {
				printf("Just about to send: ");
				packet->Print();
			}
			packet->Send(this->write_fd);
			// To not overload buffers on the robot, hold off a little bit
			//if (debug_mode)
			//	printf("Just sent it, I guess\n");
			// To not overload buffers on the robot, hold off a little bit
			usleep(15000);
		}
		delete(packet);
	}
}
void* Erratic::SendThreadDummy(void *driver) {
	((Erratic*)driver)->SendThread();
	return 0;
}

// Queues for sending
void Erratic::Send(ErraticPacket *packet) {
	pthread_mutex_lock(&send_queue_mutex); {
		send_queue.push(packet);
		pthread_cond_signal(&send_queue_cond);
	} pthread_mutex_unlock(&send_queue_mutex);
}

// Tells the robot to reset the odometry center
void Erratic::ResetRawPositions() {
	ErraticPacket *pkt;
	unsigned char erraticcommand[4];

	if(this->motor_packet) {
		pkt = new ErraticPacket();
		this->motor_packet->rawxpos = 0;
		this->motor_packet->rawypos = 0;
		this->motor_packet->xpos = 0;
		this->motor_packet->ypos = 0;
		erraticcommand[0] = (command_e)reset_origo;
		erraticcommand[1] = (argtype_e)argint;

		//TODO this is removed as a hack
		//pkt->Build(erraticcommand, 2);
		//this->Send(pkt);
	}
}

// Enable or disable motors
void Erratic::ToggleMotorPower(unsigned char val) {
	unsigned char command[4];
	ErraticPacket *packet = new ErraticPacket();

	command[0] = (command_e)enable_motors;
	command[1] = (argtype_e)argint;
	command[2] = val;
	command[3] = 0;
	packet->Build(command, 4);
	
	Send(packet);
}

// Enable or disable analog input reporting
void Erratic::ToggleAIn(unsigned char val) {
	unsigned char command[4];
	ErraticPacket *packet = new ErraticPacket();

	command[0] = (command_e)set_analog;
	command[1] = (argtype_e)argint;
	command[2] = val ? 1 : 0;
	command[3] = 0;
	packet->Build(command, 4);
	
	Send(packet);	
}

// This describes the IR hardware
float Erratic::IRRangeFromVoltage(float voltage) {
	// This is values for the Sharp 2Y0A02, 150 cm ranger
	return -.2475 + .1756 * voltage + .7455 / voltage - .0446 * voltage * voltage;
}


/** Talking to the Player architecture **/

// Main entry point for the worker thread
void Erratic::Main() {
	int last_position_subscrcount=0;
	int last_aio_ir_subscriptions=0;