void CombinedMotorWidget::go()
{
move_to_position(ui->motors->currentIndex(),
ui->speed->text().toInt(),
ui->goalPos->text().toInt());
#ifdef A_KOVAN
publish();
#endif
}
/*
* Class: Motor
* Method: move_to_position
* Signature: (III)I
*/
JNIEXPORT jint JNICALL Java_cbccore_low_Motor_move_1to_1position(JNIEnv* env, jobject obj, jint port, jint speed, jint delta)
{
#ifdef CBC
return move_to_position(port, speed, delta);
#else
printf("Java_cbccore_low_Motor_move_1to_1position stub\n");
return -1;
#endif
}
task main()
{
// initialize_receiver(irrLeft, irrRight);
int i;
for (i = 1; i < 4; i++) {
move_to_position(i);
dir = get_dir_to_beacon(irr_left);
if (dir == DIR_CENTER) {
break;
}
}
move_to_beacon(irr_left, irr_right, 35, true);
}
/**
* Move a motor
*
* Used to move the arm up and down
*/
int move_motor (int number, int power, int position)
{
int loop_done = 0;
move_to_position (number, power, position);
while(! loop_done)
{
if (get_motor_done (number))
{
loop_done = 1;
}
msleep(100);
}
}
void main ()
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle0 = 0;
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
angle0 = get_create_total_angle (.1);
create_drive_direct (150,-150);
move_to_position (0, -900, -1550);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.1), task_A_done);
if (angle0 - get_create_total_angle (.1) < 90)
{
create_drive_direct (150,-150);
}
else
{
task_A_done = 1;
}
if (get_motor_done (0))
{
}
else
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(0.1);
}
create_stop ();
create_disconnect ();
//put specific commands to control robot in here
// loop_done = 0;
// task_A_done = 0;
// task_B_done = 0;
// while(! loop_done)
// {
// if (/*task A is not done*/)
// {
// //continue doing task A
// }
// else
// {
// task_A_done = 1;
// }
// if (/*task B is not done*/)
// {
// //continue doing task B
// }
// else
// {
// task_B_done = 1;
// }
// if (task_A_done && task_B_done)
// {
// loop_done = 1;
// }
// }
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_to_position (0, -900, -1200);
while(! loop_done)
{
if (get_motor_done (0))
{
loop_done = 1;
}
msleep(100);
}
create_stop ();
loop_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (50,-50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) >= 20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (-50,50);
while(! loop_done)
{
printf("a0: %d, a: %d, d: %d\n", angle0, get_create_total_angle (.05), task_A_done);
if (angle0 - get_create_total_angle (.05) <= -20)
{
loop_done = 1;
create_stop ();
}
msleep(100);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -3100);
msleep (900);
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
loop_done = 0;
create_stop ();
task_A_done = 0;
task_B_done = 0;
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
angle0 = 0;
angle0 = get_create_total_angle (.05);
create_drive_direct (70, -70);
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
//while(angle0 - get_create_total_angle (.05) < 4)
if (0)
{
printf("a0: %d, a: %d\n", angle0, get_create_total_angle(0.05));
msleep(50);
}
move_to_position (0, 400, -900);
distance0 = get_create_distance (.05);
create_drive_direct (20, 20);
while (! loop_done)
{
if (get_create_distance (.05) - distance0 > 50)
{
loop_done = 1;
create_stop ();
}
}
set_servo_position (0 ,400);
loop_done = 0;
task_A_done = 0;
task_B_done = 0;
create_drive_straight (-20);
move_to_position (0, 600, -300);
while (! get_motor_done (0))
{
msleep (50);
}
create_drive_straight (-50);
msleep (2000);
//while (! loop_done)
if (0)
{
if (digital (10))
{
create_stop ();
loop_done = 1;
}
if (get_create_distance (.05) - distance0 < -100)
{
create_stop ();
loop_done = 1;
}
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
create_drive_direct (60, -60);
while(angle0 - get_create_total_angle (.05) < 3)
{
msleep(50);
}
create_stop ();
set_servo_position (0, 1510);
move_to_position (0, -1000, -3000);
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards untill the bumper hits the transport (not completed)
printf("-----Step 6-----\n");
create_drive_direct (130, 280);
distance0 = get_create_distance (.05);
while (! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 > 700)
{
loop_done = 1;
}
msleep(50);
}
loop_done = 0;
move_to_position (0, -900, -500);
create_drive_straight (-100);
msleep (1000);
set_servo_position (0, 200);
create_disconnect ();
printf("-----Step 6-B-----\n");
create_drive_direct (-400, -150);
distance0 = get_create_distance (.05);
while(! loop_done)
{
printf("d0: %d, d: %d, d: %d\n", distance0, get_create_distance (.05), loop_done);
if (get_create_distance (.05) - distance0 < -100)
{
loop_done = 1;
create_stop ();
}
msleep(50);
}
loop_done = 0;
// ------------------------------------------------------------------------
//step:7 drop botguy.
printf("-----Step 7-----\n");
move_to_position (0, -900, -300);
msleep (6000);
set_servo_position (0, 200);
create_disconnect ();
}
int main(int argc, char **argv)
{
printf("ur5 Server Start\n");
// JointVector tcp_home= {0.0, -191.0, 600.0, 0.0, -2.2211, -2.2211};
JointVector joint_home_rad={0.0000, -1.5708, 0.0000, -1.5708, 0.0000, 0.0000};
JointVector joint_home={0.0000, -90, 0.0000, -90, 0.0000, 0.0000};
JointVector init_two ={-6.5018, -95.3469, -29.8123, -194.7758, -59.0822, -62.3032};
JointVector init_right_side = {0.309199, -1.240009, 0.323497, -1.039543, 0.541075, 0.486453};
JointVector initialP_rad = {-0.330294, -1.881878, -0.290919, -2.243194, -0.660115, -0.704284};
JointVector viereck[4] = {{43.24, -140.88, -30.05, -12.83, 129.28, -0.27},
{130.67, -140.87, -30.05, -12.75, 44.08, -0.27},
{130.67, -187.90, 6.46, -1.22, 43.35, -0.27},
{43.24, -187.90, -30.05, 3.87, 100.32, -0.27}
};
JointVector PosOne= {36.12, -112.88, -8.78, -24.53, 88.39, 16.64};
int i,h,n=0;
for(i=0; i<4;i++){
for(h=0;h<6;h++){
viereck[i][h]= deg_to_rad(viereck[i][h]);
}
}
for(i=0;i<MSG_BUFFER_SIZE;i++) msg_buffer[i].text = msg_text_buffers[i];
// JointVector initialP={-30.4347, -132.2621, -40.8718, -149.8236, -59.1151, -62.2781};
// int i;
// printf("joint_home ={");
// for(i=0; i<6; i++){
// joint_home_rad[i]= deg_to_rad(joint_home[i]);
// joint_home[i] = rad_to_deg(joint_home_rad[i]);
// printf("%3.4f%s",joint_home_rad[i], i<5 ? ", " : "};");
//// }
// MovePTPPacket move_ptp_packet;
for(i=0; i<6; i++){
PosOne[i]= deg_to_rad(PosOne[i]);}
// move_ptp(joint_home_rad, PosOne , &move_ptp_packet);
// return 0;
struct connection_data server;
fd_set masterfds, readfds;
struct timeval timeout,send_time_start, send_time_end;
int rc;
pthread_t tcp_server_thread;
MovePTPPacket move_ptp_packet;
server.initialize_direction = -1.0;
read_args(argc, argv, &server);
initialize_direction = server.initialize_direction;
printf("initialize direction: %f", initialize_direction);
rc = pthread_create(&tcp_server_thread, NULL, &start_tcp_server, &server);
if(rc < 0){
quit_program=true;
}
int connection_timeout_count=0;
pva_packet.header.protocol_id=PVA_PACKET_ID;
p_packet.header.protocol_id=PPACKET_ID;
//--------------------------- Init Robot -------------------------------------------------------
configuration_load();
open_interface();
power_up();
set_wrench();
if(!initialize(0)){
puts("could not initialize robot");
exit(EXIT_FAILURE);
}
settle();
setup_sigint();
setup_sigpipe();
memcpy(&last_pva_packet, &pva_packet,sizeof(PVAPacket));
for(i=0;i<6;i++) last_pva_packet.acceleration[i] = 0.0;
for(i=0;i<6;i++) last_pva_packet.velocity[i] = 0.0;
bool timeout_flag=false;
char *buff[sizeof(PVAPacket)];
//------------------------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------------------------
// for(pva_packet.header.cycle_number =0; quit_program == false; pva_packet.header.cycle_number++) {
// robotinterface_read_state_blocking();
// pthread_mutex_lock(&connect_mutex);
// if(connection_timeout_count > TIMEOUT_TOLERANCE){
// connection_timeout_count=0;
// connected=false;
// }
// //--------------------------- Stuff roboter does -------------------------------------------------------
// robotinterface_get_actual(servo_packet.position, servo_packet.velocity);
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- send -------------------------------------------------------------------
// if(connected){
// gettimeofday(&send_time_start,0);
// if(send(client_fd, (char*) &pva_packet , sizeof(pva_packet),0) < 1){
// connected=false;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- recieve -----------------------------------------------------------------
// timeout_flag=false;
// if(connected){
// FD_ZERO(&masterfds);
// FD_SET(client_fd, &masterfds);
// memcpy(&readfds, &masterfds, sizeof(fd_set));
// gettimeofday(&send_time_end,0);
// timeval_diff(&timeout, &send_time_start, &send_time_end);
// timeout.tv_usec = 6000 - timeout.tv_usec;
// if (select(client_fd +1, &readfds, NULL, NULL, &timeout) < 0) {
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// if(FD_ISSET(client_fd, &readfds)){
// n=recv(client_fd, buff, sizeof(buff), 0);
// if (n < 1 || n != sizeof(PVAPacket)){
// pthread_mutex_lock(&connect_mutex);
// connected=false;
// pthread_mutex_unlock(&connect_mutex);
// }
// }else{
// connection_timeout_count++;
// if(connection_timeout_count< TIMEOUT_TOLERANCE)
// timeout_flag=true;
// }
// if(!timeout_flag && connected){
// memcpy(&pva_packet, buff, n);
// if(connection_timeout_count) connection_timeout_count--;
// }
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //---------------------------- do stuff with data ------------------------------------------------------
// // TODO check for errors-----
// //------------------------------------------------------------------------------------------------------
// if(!timeout_flag && connected){
// // save pva to last if something went wrong;
// memcpy(&last_pva_packet, &pva_packet, sizeof(PVAPacket);
// }
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// //--------------------------- Stuff roboter does -------------------------------------------------------
// if(connected){
// if(!timeout){
// robotinterface_command_position_velocity_acceleration(pva_packet.position,
// pva_packet.velocity,
// pva_packet.acceleration);
// }else{ //timeout
// if(connection_timeout_count<3){
// connected=false;
// robotinterface_command_velocity(zero_vector);
// }
// // cause of timeout we take the last pva_packet
// robotinterface_command_position_velocity_acceleration(last_pva_packet.position,
// last_pva_packet.velocity,
// last_pva_packet.acceleration);
// }
// }else{
// robotinterface_command_velocity(zero_vector);
// }
// pthread_mutex_unlock(&connect_mutex);
// robotinterface_send();
// //------------------------------------------------------------------------------------------------------
// //------------------------------------------------------------------------------------------------------
// }
// // ---- quit programm
printf("shutdown robot...\n");
//---- shutdown robot ---------
// move to home ----
move_to_position(viereck[0]);
// move_to_position(joint_home_rad);
// move_to_position(PosOne);
// ---
puts("- Speeding down");
int j;
for (j = 0; j < 10; j++)
{
robotinterface_read_state_blocking();
robotinterface_command_velocity(zero_vector);
robotinterface_send();
}
puts("close robotinterface");
robotinterface_close();
puts("Done!");
printf("shutdown program\n");
return 0;
}
void main (int argc, char ** argv)
{
int loop_done = 0;
int task_A_done = 0;
int task_B_done = 0;
int angle = 0;
int dangle = 0;
int angle0 = 0;
int distance0 = 0;
int distance = 0 ;
int ddistance = 0;
// float lf_args[20];
// read inputs
// printf("Args: %d\n", argc);
// for (i = 2; i < argc; i++)
// {
// scanf("%lf%, argv[i]
// }
//get_create_total_ angle (.1);
//create_drive_direct (left speed, right speed)\
//move_to_position (motor #, motor power(-for backwards), end position)
//get_motor_done (motor#)
//clear_motor_position_counter (motor#)
//msleep (seconds)
//i'll probably use this often
//to start by light, do:
//while(analog(1) > 200)
if (0)
{
//we have seen the light
msleep(100);
if(analog(1) < 800){
shut_down_in(119);
}
}
create_connect ();
clear_motor_position_counter (0);
enable_servo (0);
set_servo_position (0 ,1300);
//set_servo_position (1800);
// ------------------------------------------------------------------------
//step:1 aim the arm to knock over the box
//raise arm
printf("-----Step 1-----\n");
move_motor (0, -900, -1200);
// swing arm to hit the box, then get back to position
spin (50, -50, 20);
spin (-50, 50, 20);
create_stop ();
// ------------------------------------------------------------------------
//step:2 knock over box and turn
//lift arm
//turn 90
printf("-----Step 2-----\n");
move_to_position (0, -900, -2600);
msleep (900);
//turn create in a circular position and raise arm so it doesn't hit anything
angle0 = get_create_total_angle (.05);
angle = angle0;
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
create_drive_direct (-178,-648);
while(! loop_done)
{
if (dangle >= 155)
{
task_A_done = 1;
create_stop ();
}
else if (dangle >= 125)
{
create_drive_direct (-120, -181);
}
if (get_motor_done (0))
{
task_B_done = 1;
}
if (task_A_done && task_B_done)
{
loop_done = 1;
}
msleep(100);
angle = get_create_total_angle (.05);
dangle = abs (angle0 - angle);
printf("a0: %d, a: %d, da: %d, d: %d\n", angle0, get_create_total_angle (.05), dangle , task_A_done);
}
create_stop ();
// ------------------------------------------------------------------------
//step:4 aim claw to point of botguy
//turn 20
//lower arm
printf("-----Step 4-----\n");
//--tweaking position so it gets in just the right position
move_to_position (0, 400, -900);
move (20, 20, 50);
set_servo_position (0 ,400);
create_drive_straight (-30);
move_motor (0, 600, -300);
create_drive_straight (-38);// at this point, botguy is in his palm
//can't remember function to program
msleep (3000);
loop_done = 0;
if (! loop_done)
{
create_stop ();
if (! digital (10))
{
move_motor (0, 400, -900);
spin (20, -20, 5);
move_motor (0, 600, -300);
create_stop ();
loop_done = 1;
}
else
{
loop_done = 1;
}
}
// ------------------------------------------------------------------------
//step:5 grab botguy and lift him up (not complete)
printf("-----Step 5-----\n");
move (-10, -10, 10);
msleep (1300);
//spin (60, -60, 5);
//close arm and grab botguy
set_servo_position (0, 1510);
msleep (400);
move_to_position (0, -1000, -3000);
//wait for him to finish
msleep (5000);
// ------------------------------------------------------------------------
//step:6 move backwards till it's in position
printf("-----Step 6-----\n");
//back up in an arc
move (130, 280, 780);
//move towards the center and lower drop height
move_to_position (0, -900, -500);
create_drive_straight (-20);
msleep (3000);
//drop botguy in transport
set_servo_position (0, 200);
create_stop ();
//start moving towards the pipe
move_motor (0, 500, -1200);
spin (50, -50, 70);
move (50, 50, 100);
spin (-50, 50, 70);
/*
if (! loop_done)
{
if (get_object_count(2) == 1)
{
create_stop ();
spin(-10, 10, 5);
create_stop ();
move(20, 20, 20);
create_stop ();
set_servo_position (0, 1400);
loop_done = 1;
}
}
loop_done = 0;
spin (10, 10, 5);
set_servo_position (0, 200);
*/
/*
spin (50, -50, 85);
move_motor (0, 400, -400);
move (-100, -100, 50);
loop_done = 0;
//close arm on pipe
set_servo_position (0, 1510);
//turn arm to face the pole
spin (-50, 50, 50);
//up, forward, then drop pipe
move_motor (0, 400, -2300);
move (-100, -100, 100);
set_servo_position (0, 200);
ao ();
*/
create_disconnect ();
}
int main(void)
{
setup();
uint16_t increment = 0;
//DDRD &= ~(1<<PD4); no longer needed
//PORTD |= (1<<PD4);
while(1)
{
/* measure supply voltage and warn user if voltage drops too low */
if(ISR_flag.check_supply) //measure supply voltage every 10ms
{
check_supply_voltage();
ISR_flag.check_supply = 0; //clear flag
}
/* poll the inputs and look for any falling edges indicating button presses or other input signals */
if(ISR_flag.check_inputs)
{
poll_inputs();
ISR_flag.check_inputs = 0; //clear flag
/* Detect a long button press by incrementing a variable every ms the button is pressed
* This is necessary to avoid false triggering due to EMI from the motor lines.
*/
if(!(DOOR_BUTTON_PINREG & (1<<DOOR_BUTTON_PIN)))
{
increment++;
}else{
increment = 0; // reset the count if the button is released.
}
}
/* check for a 1000ms long button press
* Again, this is necessary to avoid false triggering due to motor interference
*/
if(increment >= 1000)
{
increment = 0;
command.delayed_lock = 1; //activate the delayed lock sequence
delayed_lock_tick = 0; //reset the timer for the delayed lock sequence
}
/* check, if status is unknown or a reference command has been issued
* If that is the case, ignore any other commands and find the reference point first!
*
* Note, that command.reference is not implemented yet. There was no need for this until now (10/Aug/15)
*/
if( ((!status.locked) && (!status.unlocked)) || (command.reference) )
{
if((command.lock)||(command.unlock)||(command.reference))
{
if(input_status_current[6]) //find out, if gear magnet is already near the reed contact
{
find_reference(0,500,2000);
}else{
find_reference(1,500,2000);
}
command.lock = 0;
command.unlock = 0;
command.reference = 0;
command.delayed_lock = 0;
}
}else
{
/* open the door */
if (command.unlock && !command.block_unlock && !command.reference)
{
command.block_lock = 1; //prevent any other lock commands while this is active
enable_stepper(1);
if(!status.open) //Door is not open yet, move to "open" position
{
if(move_to_position(OPEN)==1) //set status.open once at "open" position
{
status.open = 1;
}
ms_100_tick = 0; //reset the 100ms tick until at "open" position
}else if(ms_100_tick<=35)
{
//wait 3.5s at "open" position before turning back to "neutral". This lets the user open the door by pushing it
}
if((ms_100_tick>35)&&(status.open)) //Door is "open" and we waited for 3.5s
{
if(move_to_position(NEUTRAL)==1) //move back to "neutral" position and reset all status and command bits.
{
command.unlock = 0;
command.block_lock = 0;
status.open = 0;
status.locked = 0;
status.unlocked = 1;
status.error = 0;
status.reached_endstop = 0;
command.delayed_lock = 0;
enable_stepper(0);
}
}
} else
/* lock the door */
if (command.lock && !command.block_lock && !command.reference)
{
/* locking is the same as a reference turn, so we use every lock command to re-reference our position
* to account for stupid users turning the gear by hand
*/
enable_stepper(1);
command.block_unlock = 1; //block any other actions while locking the door
if(!status.reached_endstop && (position > (-2000))) //turn CCW until either endstop is reached or 2000 steps have passed
{
move_to_position(-2000);
ms_100_tick = 0;
}
else if (position <= (-2000)) //2000 steps have passed without reaching the endstop -> something went wrong
{
command.lock = 0;
command.block_unlock = 0;
status.locked = 0;
status.unlocked = 0;
status.error = 1;
command.delayed_lock = 0;
enable_stepper(0);
}
else if(ms_100_tick<=1){ //endstop reached, reset the position to 0 while waiting a bit
position = 0;
}
if((ms_100_tick>3)&&(status.reached_endstop)) //return to "neutral" position after 300ms
{
if(move_to_position(NEUTRAL)==1) //reset status and command bits after coming back to the "neutral" position
{
command.lock = 0;
command.block_unlock = 0;
status.reached_endstop = 0;
status.locked = 1;
status.unlocked = 0;
status.error = 0;
enable_stepper(0);
command.delayed_lock = 0;
}
}
}
else
/* someone locked the door by hand and triggered the endstop. Set status to "locked" */
if (!command.lock && !command.unlock && !command.reference)
{
if(status.reached_endstop)
{
status.open = 0;
status.unlocked = 0;
status.locked = 1;
}
}
}
/* delayed lock has been triggered. Start the countdown */
if (command.delayed_lock && status.unlocked && !status.open && !command.block_unlock && !command.block_lock && !command.lock && !command.unlock)
{
if(delayed_lock_tick < 150) //wait for 15s and set "status.delayed_lock". This triggers the flashing yellow LED
{
status.delayed_lock = 1;
switch_LED(0,'B');
}else
{
status.delayed_lock = 0;
command.delayed_lock = 0;
command.lock = 1; //15s have passed, give the "lock" command to lock the door
}
}
/* Switch LEDs according to the status bits.
*
* This needs to be streamlined, it is way to complicated
*/
if(status.delayed_lock) //Override any other LED configurations while "status.delayed_lock" is active
{
flash_LED(25,(180-delayed_lock_tick),'Y');
switch_LED(0,'B');
}else
{
if(status.error || ( (!status.unlocked)&&(!status.locked)))
{
flash_LED(10,20,'Y');
flash_LED(10,20,'B');
set_status('E');
}
if(status.open)
{
flash_LED(25,40,'B');
switch_LED(0,'Y');
set_status('U');
}
if(status.unlocked && !status.open)
{
switch_LED(0,'Y');
switch_LED(1,'B');
set_status('U');
}
if(status.locked && !status.open)
{
switch_LED(0,'B');
switch_LED(1,'Y');
set_status('L');
}
}
}
}
int move_relative_position(int motor, int speed, int delta_pos)
{
if(motor < 0 || motor > 3) return -1;
move_to_position(motor, speed, Private::Motor::instance()->backEMF(motor) + delta_pos);
return 0;
}
int mtp(int motor, int speed, int goal_pos)
{
return move_to_position(motor, speed, goal_pos);
}
int main()
{
int stupid_int = 0;
int value = 0;
int i = 0;
int angle_of_pole = 90;
camera_open(LOW_RES);
camera_update();
create_connect();
set_create_total_angle(0);
value = get_create_total_angle();
printf("Angle is %d\n", value);
//------------------------
// part 1: spin until past all booster sections
while(get_create_total_angle() > -85){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(1,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
enable_servos();
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of_pole = 60;
//end part 3
//part 4: repeat everything
/*while(get_create_total_angle() > -80){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -70){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 50;
//end part 3
while(get_create_total_angle() > -100){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 40;
//end part 3
while(get_create_total_angle() > -60){
create_spin_CW(75);
}
//---------------------
//end part 1
camera_update();
sleep(2);
point2 mcenter = get_object_center(0,0);
set_servo_position(0,OPEN_POSITION); //this is opening the claw so it won't go to a random position when enabling the servos
//--------------------------------------
// part 2: find booster section using function i wrote at bottom of code then line up with B2(pole)
find_item();
printf("I saw a booster section!\n");
while(digital(10) == 0) {
create_drive_straight(-50);
if(digital(10) == 1){
create_stop();
}
}
while(stupid_int == 0) {
if (digital(10) == 1){
set_servo_position(0,CLOSE_POSITION_FOR_BOOSTER_SECTIONS);
stupid_int == 1;
msleep(200);
printf("I have grabbed a booster section!\n");
}
}
move_to_position(0, -900, UPPOSITION);
set_create_total_angle(0);
while(get_create_total_angle() < angle_of_pole){
create_spin_CCW(75);
}
//---------------------
//end part 2
set_create_total_angle(0);
// part 3: go forward until i find B2 (not complete)
set_servo_position(1,OPEN_POSITION);
angle_of pole = 60;
//end part 3*/
//end part 4
}
