static void set_servos(uint32_t pos1_us, uint32_t pos2_us)
{
gpio_toggle(GPIOC, GPIO13); /* LED on/off */
servo_set_position(SERVO_CH1, pos1_us);
servo_set_position(SERVO_CH2, pos2_us);
delay(45000000);
}
void handle_error()
{
state = StateDisconnected;
led_off(UART0_LED_STATE);
servo_set_position(DOCK_SERVO_ZAXIS_ID, DOCK_SERVO_POWER_OFF);
servo_set_position(DOCK_SERVO_XAXIS_ID, DOCK_SERVO_POWER_OFF);
uart0_flush();
}
void send_error(ErrorCode code, uint8_t seq)
{
const uint8_t size = (seq ? 0x06 : 0x05);
uint8_t msg[size];
state = StateDisconnected;
led_off(UART0_LED_STATE);
servo_set_position(DOCK_SERVO_ZAXIS_ID, DOCK_SERVO_POWER_OFF);
servo_set_position(DOCK_SERVO_XAXIS_ID, DOCK_SERVO_POWER_OFF);
msg[0] = IdErrorMessage;
msg[1] = size;
msg[2] = sequence++;
msg[3] = code;
if (seq)
{
msg[4] = seq;
}
msg[size - 1] = calc_checksum(msg);
uart0_write_buffer(msg, size);
}
void handle_servo_ctrl()
{
if (state != StateConnected)
{
send_error(ErrorDisconnect, message.sequence);
return;
}
if (message.data_size != 2)
{
send_error(ErrorParser, message.sequence);
return;
}
uint8_t id = message.data[0];
if (id == DOCK_SERVO_ZAXIS_ID)
{
dock_set_docktype(ManualDocking);
}
servo_set_position(id, message.data[1]);
send_ack(message.sequence);
}
/* Initialize servo module. */
void
servo_init (void)
{
/* Set-up all the pins of the servo to out direction */
AC_SERVO_DDR = 0xff;
/* All pins are at low state by default */
/* Set-up the timer/counter 2:
- prescaler 256 => 4.44 ms TOP */
TCCR2 = regv (FOC2, WGM20, COM21, COM20, WGM21, CS22, CS21, CS20,
0, 0, 0, 0, 0, 1, 0, 0);
/* The state machine start with the first servo */
servo_updating_id_ = 0;
/* Enable overflow interrupt */
set_bit (TIMSK, TOIE2);
/* By default, servo init disable all servo. */
uint8_t i;
for (i = 0; i < SERVO_NUMBER; i++)
servo_set_position (i, 0);
}
int main(void)
{
clock_init();
gpio_init();
servo_init();
gpio_set(GPIOC, GPIO12);
delay(150000000);
// let pan-til "look around a little"
while(1) {
servo_set_position(SERVO_CH1, SERVO_MIN);
servo_set_position(SERVO_CH2, SERVO_MAX);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_MAX);
servo_set_position(SERVO_CH2, SERVO_MIN);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_MIN);
servo_set_position(SERVO_CH2, SERVO_MIN);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_MAX);
servo_set_position(SERVO_CH2, SERVO_MAX);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_MIN);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_MAX);
servo_set_position(SERVO_CH2, SERVO_NULL);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_MIN);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_MAX);
delay(15000000);
servo_set_position(SERVO_CH1, SERVO_NULL);
servo_set_position(SERVO_CH2, SERVO_NULL);
delay(15000000);
}
return 0;
}
static void run(void)
{
robot_console_printf("\n\n\n");
int i;
float y[2*NB_CPG];
float dy[2*NB_CPG];
float x[NB_CPG];
float delta = 0.0;
float speed = 0.0;
float drive =0.0;
char speed_char[10];
char drive_char[10];
/*for (i = 0;i<7;i++) {
old_pos[i] = current_position[i];
}*/
/*float initial_conditions[2*NB_CPG]={0.0, 0.0, 0.0, 0.0,
0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1,
0.0, 0.0, 0.0, 0.0,
0.2, 0.1};*/
//base values
//FILE* fileID = fopen("recherche_syst.m", "w");
//fprintf(fileID, "Z = [ ");
float v;
float R[NB_CPG];
for(i=0; i<2*NB_CPG; ++i)
{
y[i] = 0.2; //initial_conditions[i]; // (float)(9-(i/2))/10.0;
dy[i] = 0.0;
}
// run TIME miliseconds
for(i = 0 ; i < TIME ; i += TIME_STEP)
{
drive=getDrive(drive);
v = drive;
set_ampl(v, R);
fcn(y, dy, v, R, current_movement);
euler(y,dy,TIME_STEP);
next_step(x,y);
//fprintf(fileID," %f %f %f %f %f %f ;\n", y[2]-y[0], y[4]-y[0], y[6]-y[2], y[6]-y[4], y[6]-y[0], y[2]-y[4]);
// Calculate speed
if (!(i%(TIME_STEP*50))) {
//Every 50 TIME_STEP to avoid oscillations (= 800ms)
delta = pow(pow((current_position[0]- old_pos[0]),2) + pow((current_position[2]-old_pos[2]),2), 0.5);
old_pos[0] = current_position[0];
old_pos[2] = current_position[2];
speed = 1000*delta/((float)50*TIME_STEP);
}
//robot_console_printf("Speed : %d, ,%f, %f, %f, %f\n", i, delta, speed, old_pos[0], current_position[0]);
elapsed_time += (float) TIME_STEP / 1000;
/*if (elapsed_time > 5.0 && abs(delta) < 0.000001) {//abs(current_position[6]) > 2) {
break;
}*/
// Print informations
sprintf(drive_char, "%.2f", drive);
sprintf(speed_char, "%.2f", speed);
supervisor_set_label(3, drive_char, 0.16, 0.01, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(4, getCurrentMovementString(), 0.16, 0.045, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(5, speed_char, 0.16, 0.08, 0.04, 0xff0000); /* red speed */
// actuate front legs
servo_set_position(servos[0], x[0]);
servo_set_position(servos[2], x[2]);
//actuate front kmees
servo_set_position(servos[5], x[4]);
servo_set_position(servos[7], x[6]);
// actuate back legs
servo_set_position(servos[1], x[1]);
servo_set_position(servos[3], x[3]);
//actuate back knees
servo_set_position(servos[6], x[5]);
servo_set_position(servos[8], x[7]);
//actuate spine
servo_set_position(servos[4], x[8]);
robot_step(TIME_STEP);
}
// Reset
servo_set_position(servos[0], 0);
servo_set_position(servos[1], 0);
servo_set_position(servos[2], 0);
servo_set_position(servos[3], 0);
servo_set_position(servos[4], 0);
servo_set_position(servos[5], 0);
servo_set_position(servos[6], 0);
servo_set_position(servos[7], 0);
servo_set_position(servos[8], 0);
const float FLOAT_POS[7] = { 0, 1000, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)FLOAT_POS);
robot_step(10*TIME_STEP);
const float INITIAL_POSITION_AND_ROTATION[7] = { 0, 0.3, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)INITIAL_POSITION_AND_ROTATION);
elapsed_time = 0.0;
//fclose(fileID);
}
static int run(int ms)
{
int i,j;
unsigned short sensors_value[MAX_SENSOR_NUMBER];
int connector_presence[MAX_CONNECTOR_NUMBER];
float speed[2];
float dxEmit, dzEmit;
char sendMessage[20]; // message that robot sends
char angleMessage[20]; // angle to turn piece
// Update the keyboard
keyboard();
// Update the distance sensors
for (i = 0; i < sensor_number; i++) {
sensors_value[i] = distance_sensor_get_value(sensors[i]);
}
// Update the connector presence sensors
for (i = 0; i < MAX_CONNECTOR_NUMBER; i++) {
connector_presence[i] = connector_get_presence(connectors[i]);
}
////robot_console_printf("Connectors : %d %d\n", connector_presence[0], connector_presence[1]);
// Lock automatically
if (!unlock && connector_presence[0] &&
!connector_status[0]) {
//Should lock
//robot_console_printf("Lock !\n");
connector_lock(connectors[0]);
connector_status[0] = LOCKED;
}
// Unlock requested
if (unlock) {
connector_unlock(connectors[0]);
connector_status[0] = UNLOCKED;
}
// Allow locking when out of presence
if (unlock && !connector_presence[0]) {
unlock= !unlock;
}
// Emitter/receiver
/* Send null-terminated robotID */
// const char *robotID = robot_get_name();
// robot_num = robotID[1];
sprintf(sendMessage, "%s%d%c%c", robot_name, state, pieceType, pieceNum);
// strcpy(sendMessage, robotID);
// strcat(sendMessage, state2char(state));
////robot_console_printf("State: %d\n", state);
// const char stateChar = state2char(state);
// const char *sendMessage = strcat(robotID, stateChar);
robot_console_printf("message: %s\n", sendMessage);
emitter_send_packet(commEmitter, sendMessage, strlen(sendMessage) + 1);
// default velocities
velBraitenberg(sensors_value, speed);
switch(state){
case SEARCH_FOR_PIECE:
// what if it picks up a piece by accident?
pieceType = '0';
pieceNum = '0';
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// what if recMessage is less than 5 indices long?
if ( (recMessage[0] == 'p' && recMessage[4]=='0') ){
// it's an unclaimed piece on the floor
pieceType = recMessage[1];
pieceNum = recMessage[3];
robot_console_printf("Piece found-------\n");
robot_console_printf("Message: %s\n", recMessage);
speed[0] = 0;
speed[1] = 0;
abortCtr = 0;
waitCtr = 0;
state = ALIGN_WITH_PIECE;
}
receiver_next_packet(commReceiver);
}
break;
case ALIGN_WITH_PIECE:
if (receiver_get_queue_length(commReceiver)) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
if (recMessage[0] == 'p' &&
recMessage[1] == pieceType && recMessage[3] == pieceNum) {
direction = receiver_get_emitter_direction(commReceiver);
dxEmit = direction[0]; // x distance from emitter of piece
dzEmit = direction[2]+1;
//robot_console_printf("showMessage: %s\n", recMessage);
if (fabs(dxEmit) > EPS_X || fabs(dzEmit) > EPS_Z) {
//robot_console_printf("dxEmit: %f dzEmit: %f\n", fabs(dxEmit), fabs(dzEmit));
////robot_console_printf("Aligning with piece\n");
//velAlign(speed); // ALTERNATIVE
if (dxEmit >= 0) { // robot rotates about center point
speed[0] = 4000;
speed[1] = -4000;
}
else {
speed[0] = -4000;
speed[1] = 4000;
}
} else {
// move forward toward piece
state = APPROACH_PIECE;
}
}
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_PIECE;
//robot_console_printf("ALIGN_WITH_PIECE: Communication broken !\n");
}
//speed[0] = 0;
//speed[1] = 0;
break;
case APPROACH_PIECE:
if (connector_status[0] == UNLOCKED) {
speed[0] = 5000;
speed[1] = 5000;
}
else {
state = ROTATE_PIECE;
}
break;
case ROTATE_PIECE:
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
//robot_console_printf("Piece message: %s\n", recMessage);
if (recMessage[1] == pieceType && recMessage[3] == pieceNum){
// the piece that it's carrying
if (recMessage[5] == 'A' && turnAngle == 0) {
sprintf(angleMessage, "%c%c%c%c%c", recMessage[6], recMessage[7], recMessage[8],
recMessage[9], recMessage[10]);
turnAngle = atof(angleMessage);
float arm_position = servo_get_position(arm_servo);
////robot_console_printf("servo : %f ", arm_position);
servo_set_position(arm_servo, arm_position+turnAngle);
//robot_console_printf("Angle message: %s\n", angleMessage);
}
if (recMessage[4] == '3') { // piece is now aligned
// NOTE: 3 is the piece state CARRIED
turnAngle = 0;
// turn a little more for certain pieces
/*if (pieceType == '3' || pieceType == '4') {
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-0.2);
} */
state = SEARCH_FOR_ROBOT;
}
}
receiver_next_packet(commReceiver);
}
speed[0] = 0;
speed[1] = 0;
break;
case SEARCH_FOR_ROBOT:
//velBraitenberg(sensors_value, speed);
/* Is there at least one packet in the receiver's queue ? */
if (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
//robot_console_printf("Message: %s\n", recMessage);
if (recMessage[0] == 'r' && char2state(recMessage[2])==SEARCH_FOR_ROBOT) {
// specific assembly plan
if ( (pieceType == '1' && recMessage[3] == '2') ||
(pieceType == '2' && recMessage[3] == '1') ||
(pieceType == '3' && recMessage[3] == '4') ||
(pieceType == '4' && recMessage[3] == '3') ||
(pieceType == '5' && recMessage[3] == '6') ||
(pieceType == '6' && recMessage[3] == '5') ||
(pieceType == '2' && recMessage[3] == '7') ||
(pieceType == '7' && recMessage[3] == '2')) {
robot_console_printf("Robot nearby------\n");
speed[0] = 0;
speed[1] = 0;
if (pieceType == '2' && recMessage[3] == '7') {
// rotate piece 2 in new position to fit with piece 7
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-3.14);
}
state = ALIGN_WITH_ROBOT;
}
}
receiver_next_packet(commReceiver);
}
if (pieceType == '7') {
//robot_console_printf("Holding piece 7\n");
// speed[0] = 0;
// speed[1] = 0;
}
break;
case ALIGN_WITH_ROBOT:
if (receiver_get_queue_length(commReceiver) > 0) {
// other robot is still in communication range
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// MORE CONDITIONS
if (recMessage[0] == 'r'){ // it's a robot (not a piece)
if (robot_num < recMessage[1]) {
robot1 = robot_num;
robot2 = recMessage[1];
}
else {
robot1 = recMessage[1];
robot2 = robot_num;
}
/* print null-terminated robotID */
//robot_console_printf("Message from nearby robot: \"%s\"\n", recMessage);
////robot_console_printf("Other robot is in state: %d\n",char2state(recMessage[2]));
if (robot_num == robot2 && char2state(recMessage[2])==ALIGN_WITH_ROBOT) {
// robot 2 waits while robot 1 aligns
speed[0] = 0;
speed[1] = 0;
state = WAIT;
//robot_console_printf("Robot is R2, waiting\n");
}
if (robot_num == robot1 || (robot_num == robot2 && char2state(recMessage[2])==WAIT)) {
////robot_console_printf("Robot is R1, aligning\n");
direction = receiver_get_emitter_direction(commReceiver);
//robot_console_printf("Direction: [%f %f %f]\n", direction[0], direction[1], direction[2]);
dxEmit = direction[0]; // x distance from emitter of other robot
dzEmit = direction[2]+1;
// TO DO: replace this with more general code for multiple robots
if (fabs(dxEmit) > EPS_X || fabs(dzEmit) > EPS_Z) {
//robot_console_printf("dxEmit: %f dzEmit: %f\n", fabs(dxEmit), fabs(dzEmit));
if (dxEmit >= 0) {
speed[0] = 4000;
speed[1] = -4000;
}
else {
speed[0] = -4000;
speed[1] = 4000;
}
//velAlign(speed); // alternative
} else {
speed[0] = 0;
speed[1] = 0;
if (robot_num == robot1) {
state = WAIT; // wait for robot 2 to align
}
if (robot_num == robot2) {
state = APPROACH_ROBOT; // start approach to robot 1
}
}
}
}
/* fetch next packet */
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_ROBOT;
//robot_console_printf("ALIGN_WITH_ROBOT: Communication broken !\n");
}
break;
case WAIT:
// what if it receives other robots' messages?
if (receiver_get_queue_length(commReceiver) > 0) {
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
speed[0] = 0;
speed[1] = 0;
if (robot_num == robot2 && char2state(recMessage[2])==WAIT) {
state = ALIGN_WITH_ROBOT; // robot 1 has finished aligning
}
if (robot_num == robot1 && char2state(recMessage[2])==APPROACH_ROBOT) {
state = APPROACH_ROBOT;
}
/* fetch next packet */
receiver_next_packet(commReceiver);
}
else {
state = SEARCH_FOR_ROBOT;
//robot_console_printf("WAIT: Communication broken !\n");
}
break;
case APPROACH_ROBOT:
speed[0] = 3000;
speed[1] = 3000;
if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
const void *buffer = receiver_get_data(commReceiver);
const char *recMessage = (char*) buffer;
// pieces have locked together; numbers are the resulting
// conglomerate piece states
if (strcmp(recMessage,"Lock5")==0 ||
strcmp(recMessage,"Lock6")==0 ||
strcmp(recMessage,"Lock7")==0 ||
strcmp(recMessage,"Lock8")==0 ){
// the piece that it's carrying
speed[0] = 0;
speed[1] = 0;
// pieceType of robot not carrying piece will later go back to '0'
pieceTypeNew = recMessage[4];
state = SEPARATE_ROBOTS;
}
receiver_next_packet(commReceiver);
}
break;
case SEPARATE_ROBOTS:
//speed[0] = 0;
//speed[1] = 0;
//if (receiver_get_queue_length(commReceiver) > 0) {
/* Read current packet's data */
// const void *buffer = receiver_get_data(commReceiver);
// const char *recMessage = (char*) buffer;
// if (recMessage[0] == 'r' &&
// char2state(recMessage[2])==SEPARATE_ROBOTS) {
if (ctr == 0 &&
(pieceType == '2' || pieceType == '3' || pieceType == '5')) {
// detach from magnet
robot_console_printf("Detach-------\n");
unlock = true;
}
if (ctr < 25) { //back up
speed[0] = -8000;
speed[1] = -8000;
ctr += 1;
}
else if (ctr >= 20 && ctr < 45) { // turn away from other robot
speed[0] = -8000;
speed[1] = 8000;
ctr += 1;
}
else {
ctr = 0;
if (pieceType == '1' || pieceType == '4') { // ||
//pieceType == '6') { // hold onto piece
pieceType = pieceTypeNew;
state = SEARCH_FOR_ROBOT;
}
else if (pieceType == '6') {
pieceType = pieceTypeNew;
float arm_position = servo_get_position(arm_servo);
servo_set_position(arm_servo, arm_position-1.57);
state = SEARCH_FOR_ROBOT;
}
else if (pieceType == '2' || pieceType == '3' ||
pieceType == '5') {
state = SEARCH_FOR_PIECE;
}
else if (pieceType == '7') { // puzzle is completed
unlock = true;
state = SEARCH_FOR_PIECE;
}
}
//}
//receiver_next_packet(commReceiver);
//}
break;
}
/* Set the motor speeds */
//robot_console_printf("Speeds: %d %d\n", (int) speed[0], (int) speed[1]);
differential_wheels_set_speed((int) speed[0], (int) speed[1]);
// Output the current metrics into the file
//file_output();
elapsed_time += (float)TIME_STEP / 1000.0;
return TIME_STEP; /* run for TIME_STEP ms */
}
/**
* Move a servo to a specific position.
* @param servo_id the id of the servo to move.
* @param position the position identifier where to move the servo.
*/
void
servo_pos_move_to (uint8_t servo_id, uint8_t position)
{
servo_set_position (servo_id, servo_pos_high_time[servo_id][position]);
}
void set_servo1_position(char* arg, uint8_t arglen)
{
int16_t pos = atoi(arg);
servo_set_position(1, pos);
}
