/**************************************************************************** * * Copyright (c) 2008 www.societyofrobots.com * (please link back if you use this code!) * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License version 2 as * published by the Free Software Foundation. * * Alternatively, this software may be distributed under the terms of BSD * license. * ****************************************************************************/ //Add your code here //enter your code in control here //photovore is just a default sample program and can be deleted char cByte; char cByte; char dByte; char eByte; int modify_servo = 1; int sensitivity = 10; //change this value to move servos in smaller increments. int speed = 1; //walking speed int repeat = 0; int i = 0; void control(void) { rprintfInit(uart1SendByte); delay_ms(50); welcome_message(); //initial message displayed to user delay_ms(50); while(1) { cByte=uart1GetByte(); //get byte from USB on the axon if(cByte!=-1)//no new data { if (cByte == 'o') LED_on(); if (cByte == 'p') LED_off(); if (cByte == 'h') { rprintf("\n Home position\n"); home_position(); } if (cByte == 'j') jog_command(); if (cByte == 'w') walk_forward_command(); if (cByte == 's') { rprintf("\n Walking reverse \n"); walk_reverse(50);//higher number slows gait down } if (cByte == 'm') modify_servo_command(); delay_ms(100); } delay_ms(100); } } void welcome_message() { rprintf("\nSELECT FROM THE FOLLOWING OPTIONS\n"); rprintf("\n(o) LED on, \t\t(p) LED off"); rprintf("\n(h)ome position, \t(w)alk forward, \t(s) walk reverse"); rprintf("\n(m)odify servos\n"); delay_ms(100); } void modify_servo_message() { rprintf("\nWhich servo would you like to move?\n"); rprintf("0:left foot \t 1: right foot\n"); rprintf("2:left ankle \t 3: right ankle\n"); rprintf("4:left knee \t 5: right knee\n"); rprintf("6:left thigh \t 7: right thigh\n"); rprintf("8:left hip \t 9: right hip\n"); rprintf("\n(p)rint all servo values to screen\n"); rprintf("\n\nWARNING: Your robot will jolt into the home position \n"); delay_ms(100); } void modify_joint_message() { rprintf("+/- increment/decrement \t(d)one \n"); delay_ms(50); } void modify_finish_message() { rprintf("\n Finished modifying current servo. Select another option\n"); delay_ms(50); } void walk_forward_command() { speed = -1; rprintf("what speed?\n"); while(speed == -48 || speed == -1) { speed = (uart1GetByte() - 47); delay(50); } rprintf("\n Walking forward \n"); walk_forward(speed);//higher number slows gait down } void jog_command() { repeat = 1; speed = -1; rprintf("jog forward at what speed?\n"); while(speed == -48 || speed == -1) { speed = (uart1GetByte() - 47); } while(repeat) { cByte = uart1GetByte(); if (cByte == 27) repeat = 0; jog_forward(speed); } } void modify_servo_command() { modify_servo_message(); while(1) { delay_ms(100); modify_servo = 1; cByte = uart1GetByte(); //get byte from USB on the axon; if(cByte != -1)//no new data { modify_servo = 1; if (cByte == '0') { modify_left_foot(); modify_servo_message(); } if (cByte == '1') { modify_right_foot(); modify_servo_message(); } if (cByte == '2') { modify_left_ankle(); modify_servo_message(); } if (cByte == '3') { modify_right_ankle(); modify_servo_message(); } if (cByte == '4') { modify_left_knee(); modify_servo_message(); } if (cByte == '5') { modify_right_knee(); modify_servo_message(); } if (cByte == '6') { modify_left_thigh(); modify_servo_message(); } if (cByte == '7') { modify_right_thigh(); modify_servo_message(); } if (cByte == '8') { modify_left_hip(); modify_servo_message(); } if (cByte == '9') { modify_right_hip(); modify_servo_message(); } if (cByte == 'p') { print_servo_values(); modify_finish_message(); } if (cByte == 27) exit; delay_ms(300); } } } void modify_left_foot() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_left_foot(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_left_foot(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_right_foot() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_right_foot(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_right_foot(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_left_ankle() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_left_ankle(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_left_ankle(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_right_ankle() { while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_right_ankle(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_right_ankle(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_left_knee() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_left_knee(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_left_knee(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_right_knee() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_right_knee(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_right_knee(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_left_thigh() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_left_thigh(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_left_thigh(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_right_thigh() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_right_thigh(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_right_thigh(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_left_hip() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_left_hip(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_left_hip(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } } void modify_right_hip() { modify_joint_message(); while(modify_servo) { cByte = uart1GetByte(); // get byte from USB on the axon if (cByte == '+') { set_right_hip(sensitivity); delay_ms(10); biped_move(); } if (cByte == '-') { set_right_hip(-sensitivity); delay_ms(10); biped_move(); } if (cByte == 'd') { delay_ms(100); modify_finish_message(); modify_servo = 0; } delay_ms(100); } }