void executeCB(const tilting_servo::servoGoalConstPtr &goal)
{
if(start == true)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
start = false;
}
if(as_.isNewGoalAvailable())
as_.acceptNewGoal();
if(goal->angle != prev_goal || goal->speed != prev_speed)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
prev_speed = goal->speed;
}
while((as_.isNewGoalAvailable() == false) && ros::ok())
{
feedback_.angle = read_angle(Comport, servo_id);
if(feedback_.angle >= min_angle - 10 && feedback_.angle <= max_angle + 10)
as_.publishFeedback(feedback_);
}
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
// set the action state to preempted
as_.setPreempted();
}
}
void loop(void)
{
servo_move(SERVO1, 100);
Sleep(1000);
servo_move(SERVO1, 200);
Sleep(1000);
}
void loop(void)
{
int position;
printf("Enter servo position (100-200): ");
scanf("%i", &position);
printf("%i\n", position);
servo_move(SERVO1, position);
}
/**
* Initialize servos
* @return 0 upon success, 1 otherwise
*/
int servo_init() {
//Open the file regarding memory mapped IO to write values for the FPGA
gServos.fd = open( "/dev/mem", O_RDWR);
unsigned long int PhysicalAddress = BASE_ADDRESS;
gServos.map_len= 0xFF; //size of mapping window
// map physical memory startin at BASE_ADDRESS into own virtual memory
gServos.test_base = (unsigned char*)mmap(NULL, gServos.map_len, PROT_READ | PROT_WRITE, MAP_SHARED, gServos.fd, (off_t)PhysicalAddress);
// did it work?
if(gServos.test_base == MAP_FAILED) {
perror("Mapping memory for absolute memory access failed -- Test Try\n");
return 1;
}
//Initialize all servo motors to middle position, go there fast
servo_move(0, 150, 100);
servo_move(1, 150, 100);
servo_move(2, 150, 100);
servo_move(3, 150, 100);
servo_move(4, 150, 100);
servo_move(5, 150, 100);
return 0;
}
void main()
{
servo_init(no_of_servos); //initialize servo control
interrupt_init(); //initialize interrupts
unsigned char i;
while(1)
{
//move from 0deg to 180deg in 20deg steps
for(i=0; i<181; i+=20)
{
servo_move(i, 1);
Delay_ms(1000);
}
}
}
void main()
{
timer_init(); //initialize timers
TRISD=0xFE; //RD0 is output
interrupt_init(); //initialize interrupts
unsigned char i;
while(1)
{
//move from 0deg to 180deg in 10deg steps
for(i=0; i<181; i+=10)
{
servo_move(i);
Delay_ms(1000);
}
}
}
/**
* Deinitialize Servos
*/
void servo_release(){
servo_move(0, 150, 100);
servo_move(1, 150, 100);
servo_move(2, 150, 100);
servo_move(3, 150, 100);
servo_move(4, 150, 100);
servo_move(5, 150, 100);
// Releasing the mapping in memory
munmap((void *)gServos.test_base, gServos.map_len);
close(gServos.fd);
}
static QState execute_cmd(struct mmi_ao *me, int cmd)
{
vec2f_t v;
switch (cmd) {
case CMD_SINGLE_SHOT:
QActive_post((QActive *) &shutter_ao, SIG_SHUTTER_START, 0);
break;
case CMD_SPHERICAL_PAN:
return Q_TRAN(mmi_spherical_pan);
case CMD_GIGA_PAN:
return Q_TRAN(mmi_giga_pan);
case CMD_TIMELAPSE:
QActive_post((QActive *) &prog_ao, SIG_PROG_START, PROG_TIMELAPSE);
QActive_post((QActive *) &mmi_ao, SIG_PROG_START, PROG_TIMELAPSE);
break;
case CMD_SAVE:
show_msg("Saving...", 1);
param_save();
break;
case CMD_SERVO_MIN:
vec2(&v, deg2rad(0.0), deg2rad(0.0));
servo_move(&v);
break;
case CMD_SERVO_CENTER:
vec2(&v, deg2rad(180.0), deg2rad(90.0));
servo_move(&v);
break;
case CMD_SERVO_MAX:
vec2(&v, deg2rad(360.0), deg2rad(180.0));
servo_move(&v);
break;
case CMD_UPDATE_SERVO_MIN_X:
servo_get_goal(&v);
v.x = deg2rad(0);;
servo_move(&v);
break;
case CMD_UPDATE_SERVO_MAX_X:
servo_get_goal(&v);
v.x = deg2rad(360);
servo_move(&v);
break;
case CMD_UPDATE_SERVO_MIN_Y:
servo_get_goal(&v);
v.y = deg2rad(0);
servo_move(&v);
break;
case CMD_UPDATE_SERVO_MAX_Y:
servo_get_goal(&v);
v.y = deg2rad(180);
servo_move(&v);
break;
case CMD_UPDATE_CENTER:
vec2(&v, deg2rad(pd.single.center_x), deg2rad(pd.single.center_y));
servo_move(&v);
break;
case CMD_UPDATE_GIGA_START:
vec2(&v, deg2rad(pd.giga.start_x), deg2rad(pd.giga.start_y));
servo_move(&v);
break;
case CMD_UPDATE_GIGA_END:
vec2(&v, deg2rad(pd.giga.end_x), deg2rad(pd.giga.end_y));
servo_move(&v);
break;
case CMD_SHUTDOWN:
vec2(&v, deg2rad(180.0), deg2rad(180.0));
servo_move(&v);
break;
}
return Q_HANDLED();
}
int main()
{
//Servo variables
int servo_number = 0;
long position = 0;
int prevPosn = 150;
int speed = 10;
//Wiimote variables
tWiiMoteButton button;
tWiiMoteAccel accel;
unsigned char accelX =0;
unsigned char accelY =0;
int buttonValue = 0;
// Initialize wiimote
if (wiimote_init() != 0) {
printf("Failed to init WiiMote\n");
return -1;
}
printf("\n------------- ATTENTION ROBOT WILL BE MOVING! --------------------\n\n");
printf("Please ensure robot power is OFF. Hold it in middle position. Then, turn it on.\n");
sleep(1);
/* initialize servos */
if (servo_init() != 0) {
return -1; // exit if init fails
}
do {
// read acceleration (blocking)
accel = wiimote_accelGet();
// read button events that have accumulated since then.
button = wiimote_buttonGet();
position = ((accel.value * 18) / 1000) + 150;
switch (button.code) {
case A:
servo_number = 1;
buttonValue = 1;
break;
case B:
servo_number = 2;
buttonValue = 1;
break;
case ONE:
servo_number = 3;
buttonValue = 1;
break;
case TWO:
servo_number = 4;
buttonValue = 1;
break;
case DOWN:
servo_number = 5;
buttonValue = 1;
break;
default:
buttonValue = 0;
break;
}
// did we get an accel event?
if (accel.code != 0) { // ignore the zeroes
if( buttonValue && (accel.code == WIIMOTE_EVT0_ACCEL_X)) {
servo_move(servo_number, prevPosn += position, speed);
//printf("%d, %d \n", position, servo_number);
}
}
// repeat until "Home" button is pressed (or relased)
} while(button.code != HOME);
wiimote_close();
servo_release();
return 0;
}
