void sweep(int *stuff) // takes a pointer to structure containg 3 dimmensional array, sweeps back and forth
{ // moving servo first up, then down, and reading for each value
// reads a "LENGHT" number of sweeps before returning
int j = 0;
move_servo(10);
for(j = 0; j < STEPS; j++)
{
move_servo(10 + j);
_delay_ms(50);
stuff[j] = readadc(0);
//sprintf(uartBuffer, "%d %d \n" , set(stuff[j]), j);
//sendstring(uartBuffer);
}
for (; j >= 0; j--)
{
move_servo(10 + j);
_delay_ms(50);
stuff[j] = (stuff[j] + readadc(0))/2;
stuff[j] = set(stuff[j]);
//sprintf(uartBuffer, "%d %d \n" , set(stuff[j]), j);
//sendstring(uartBuffer);
}
}
int main()
{
servo arm = build_servo(1, 0, 2048);
//Build a 'servo' named 'arm'
//It is in Port #1, Its Minimum Position is 0, Its Maximum Position is 2048
servo claw = build_servo(3, 300, 1500);
//Build a 'servo' named 'claw'
//It is in Port #3, Its Minimum Position is 300, Its Maximum Position is 1500
servo_movement up = build_servo_movement(100, 15, 6);
//Build a 'servo_movement' named 'up'
//Its desired position is 100 Ticks, It moves 15 ticks each movement with a Latency of 6 ms
servo_movement down = build_servo_movement(2000, 8, 10);
//Build a 'servo_movement' named 'down'
//Its desired position is 2000 Ticks, It moves 8 ticks each movement with a Latency of 10 ms
servo_movement open = build_servo_movement(400, 20, 5);
//Build a 'servo_movement' named 'open'
//Its desired position is 400 Ticks, It moves 20 ticks each movement with a Latency of 5 ms
servo_movement close = build_servo_movement(1400, 12, 6);
//Build a 'servo_movement' named 'close'
//Its desired position is 1400 Ticks, It moves 12 ticks each movement with a Latency of 6 ms
move_servo(arm, up);
//Move the 'arm' servo to the 'up' position
move_servo(claw, open);
//Move the 'claw' servo to the open position
//NOTE: The 'claw' and 'arm' servos do not wait to finish
//NOTE: This code would immediately execute the 'drive_to_botguy()' function
//NOTE: The 'claw' and 'arm'would move while the robot is still moving
drive_to_botguy();
//Drive the robot to botguy
wait_servo(claw, close);
//Wait until the 'claw' servo is at the 'close' position
//NOTE: This function waits until the the servo is done moving to execute the next function
//NOTE: This is the same as calling 'move_servo(claw, close);' and then 'bsd(claw);'
drive_to_scoring_area();
//Drive to the scoring area
wait_servo(arm, down);
//Wait until the 'arm' servo is at the 'down' position
wait_servo(claw, open);
//Wait until the 'claw' servo is at the 'open' position
return 0;
//Returns '0' to the system
}
int main() {
//initialize all necessary sensors and utilities
lcd_init();
timer1_init();
timer3_init();
move_servo(90);
ADC_init();
USART_Init(MYUBRR);
init_push_buttons();
oi_t *sensor_data = oi_alloc();
oi_init(sensor_data);
audioInit(sensor_data);
//oi_play_song(1);
while(1) {
//empty currentObjects before proceeding by setting all stored objects to "invalid" - ignored by later checks
for (int i = 0; i < 20; i++) {
currentObjects[i].isValid = 0;
}
char received = serial_getc(); //take keyboard input from putty
takeDirectionInput(received, currentObjects); //translate keyboard input into functionality
}
return 0;
}
int axis_press(int axis, int value)
{
switch(axis)
{
//turn platform
case 0:
//left turn
if(value >= 3000 && value <= 32767){
parseCommand('A', (char)((double)value*0.0006718 + 12.987), 9);
}
//right turn
else if(value <= -3000 && value >= -32767){
parseCommand('A', (char)((double)(abs(value))*0.0006718 + 12.987), 6);
//parseCommand('A', '"', 6);
}
else{
parseCommand('A', 0, 0); //stop motor
printf("stop\n");
}
printf("LH @ %d\n", value);
break;
//move motors forward / backward
case 1:
if(value >= 3000 && value <= 32767)
if
parseCommand('A', (char)((double)value*0.0006718 + 12.987), 10); // dividing by 130 keeps 32767 within range of char
else if(value < -3000 && value >= -32767)
parseCommand('A', (char)((double)(abs(value))*0.0006718 + 12.987), 5);
else
parseCommand('A', 0, 0); //stop motor
printf("LV @ %d\n", value);
break;
// Move the stepper motor left and right
case 2:
if(value > 1000 && value < 16000)
parseCommand('G', 1, 0);
else if(value > 16000)
parseCommand('G', 2, 0);
else if(value < -1000 && value > -16000)
parseCommand('G', -1, 0);
else if(value < -16000)
parseCommand('G', -2, 0);
break;
// Move servo up and down
case 3:
//printf("RV @ %d\n", value);
move_servo(value);
//printf("servo: %d\n", servo_position);
//sprintf(position,"%c", servo_position);
parseCommand('K', servo_position, 0);
break;
case 4:
printf("DH @ %d\n", value);
break;
case 5:
printf("DV @ %d\n", value);
break;
}
void servo_on() {
TCCR3A = (1 << COM3A1) | (1 << COM3B1) | (1 << WGM31);
//TCCR3A = (1 << COM3A1) | (1 << COM3A0) | (1 << WGM31) | (1 << WGM30);
//TCCR3A = (1 << COM3A0); // orig
// COM3A0: Toggle PB6 on compare match between counter and OCR3A
TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31) | (1 << CS30);
//TCCR3B = (1 << WGM32) | (1 << CS31) | (1 << CS30); // orig
// WGM32: When counter (TCNT3) matches OCR3A, reset counter
// CS31 & CS30: Set a prescaler of 64
move_servo(0,0);
}
int main(void)
{
//lprintf("%d", 4);
init_ADC();
init_timer3();
init_timer();
init_USART(calcUBRR(BAUD));
char * str = "Degrees\tIR Distance (cm)\tSonar Distance (cm)\n\n\r";
move_servo(0);
char degrees = 0;
while(1)
{
//send_pulse();
Transmit_String(str);
sprintf(str, "%d\t%d\t%d\n\r", degrees, ir_distance(), time2dist(ping_read()));
move_servo(degrees);
degrees += 2;
if(degrees > 180)
break;
}
}
/**
* Initializes everything on the robot
* @author Group B1
* @param oi The open interface of the robot
* @date 12/4/2012
*/
void init_all(oi_t *oi)
{
oi = oi_alloc();
oi_init(oi);
init_buttons();
init_usart();
lcd_init();
timer3_init();
ADC_init();
init_printf(0,write_one_char);
move_servo(0);
wait_ms(1000);
printf("\n");
printf("\n");
}
/// This one should be called periodically
void AP_Mount::update_mount_position()
{
#if MNT_RETRACT_OPTION == ENABLED
static bool mount_open = 0; // 0 is closed
#endif
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
{
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
Vector3f vec = _retract_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
#endif
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_tilt_angle = vec.y;
_pan_angle = vec.z;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
Vector3f vec = _control_angles.get();
_roll_control_angle = radians(vec.x);
_tilt_control_angle = radians(vec.y);
_pan_control_angle = radians(vec.z);
stabilize();
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
#if MNT_JSTICK_SPD_OPTION == ENABLED
if (_joystick_speed) { // for spring loaded joysticks
// allow pilot speed position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
//_roll_control_angle += angle_input(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max) * 0.00001 * _joystick_speed;
_roll_control_angle += rc_ch[_roll_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_roll_control_angle < radians(_roll_angle_min*0.01)) _roll_control_angle = radians(_roll_angle_min*0.01);
if (_roll_control_angle > radians(_roll_angle_max*0.01)) _roll_control_angle = radians(_roll_angle_max*0.01);
}
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
//_tilt_control_angle += angle_input(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max) * 0.00001 * _joystick_speed;
_tilt_control_angle += rc_ch[_tilt_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_tilt_control_angle < radians(_tilt_angle_min*0.01)) _tilt_control_angle = radians(_tilt_angle_min*0.01);
if (_tilt_control_angle > radians(_tilt_angle_max*0.01)) _tilt_control_angle = radians(_tilt_angle_max*0.01);
}
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
//_pan_control_angle += angle_input(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max) * 0.00001 * _joystick_speed;
_pan_control_angle += rc_ch[_pan_rc_in-1]->norm_input() * 0.00001 * _joystick_speed;
if (_pan_control_angle < radians(_pan_angle_min*0.01)) _pan_control_angle = radians(_pan_angle_min*0.01);
if (_pan_control_angle > radians(_pan_angle_max*0.01)) _pan_control_angle = radians(_pan_angle_max*0.01);
}
} else {
#endif
// allow pilot position input to come directly from an RC_Channel
if (_roll_rc_in && (rc_ch[_roll_rc_in-1])) {
_roll_control_angle = angle_input_rad(rc_ch[_roll_rc_in-1], _roll_angle_min, _roll_angle_max);
}
if (_tilt_rc_in && (rc_ch[_tilt_rc_in-1])) {
_tilt_control_angle = angle_input_rad(rc_ch[_tilt_rc_in-1], _tilt_angle_min, _tilt_angle_max);
}
if (_pan_rc_in && (rc_ch[_pan_rc_in-1])) {
_pan_control_angle = angle_input_rad(rc_ch[_pan_rc_in-1], _pan_angle_min, _pan_angle_max);
}
#if MNT_JSTICK_SPD_OPTION == ENABLED
}
#endif
stabilize();
break;
}
#if MNT_GPSPOINT_OPTION == ENABLED
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_gps->fix) {
calc_GPS_target_angle(&_target_GPS_location);
stabilize();
}
break;
}
#endif
default:
//do nothing
break;
}
#if MNT_RETRACT_OPTION == ENABLED
// move mount to a "retracted position" into the fuselage with a fourth servo
bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT ? 0 : 1;
if (mount_open != mount_open_new) {
mount_open = mount_open_new;
move_servo(_open_idx, mount_open_new, 0, 1);
}
#endif
// write the results to the servos
move_servo(_roll_idx, _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1);
move_servo(_tilt_idx, _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1);
move_servo(_pan_idx, _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1);
}
/**
* Runs the main part of the program. Note it is an infinite loop.
* @author Group B1
* @date 12/4/2012
*/
void run_robot(oi_t *oi)
{
int x = STARTING_X_POS;
int y = STARTING_Y_POS;
int o = STARTING_ORENTATION;
int msg = 0;
while(1){
char c = read_one_char();
oi_update(oi);
printf("11 %d\n", oi->virtual_wall);
lprintf("\n Group B1\n\n\n\n");
lprintf("%d %d %d %d\n%d", oi->cliff_left_signal , oi->cliff_frontleft_signal , oi->cliff_frontright_signal , oi->cliff_right_signal, oi->virtual_wall );
if( oi->cliff_left_signal > 1000 && oi->cliff_frontleft_signal > 1000 && oi->cliff_frontright_signal > 1000 && oi->cliff_right_signal > 1000 ){
printf("10\n");
}
if(c == FORWARD)
{
if(move_forward(oi, DEFAULT_DISTANCE) >= DEFAULT_DISTANCE){
if(o == NORTH){
y = y + 1 * STEP;
}
else if(o == EAST){
x = x + 1 * STEP;
}
else if(o == SOUTH){
y = y - 1 * STEP;
}
else if(o == WEST){
x = x - 1 * STEP;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
}
else if(c == RIGHT_TURN)
{
turn_clockwise();
if(o == NORTH){
o = WEST;
}
else if(o == EAST){
o = NORTH;
}
else if(o == SOUTH){
o = EAST;
}
else if(o == WEST){
o = SOUTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == LEFT_TURN)
{
turn_counter_clockwise();
if(o == NORTH){
o = EAST;
}
else if(o == EAST){
o = SOUTH;
}
else if(o == SOUTH){
o = WEST;
}
else if(o == WEST){
o = NORTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == RESET)
{
move_servo(0);
x = STARTING_X_POS;
y = STARTING_Y_POS;
o = STARTING_ORENTATION;
msg = 0;
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == MUSIC)
{
load_song();
oi_play_song(JAWS);
}
else if(c == SCAN)
{
oi_update(oi);
printf("1 %d %d %d %d %d ", oi->virtual_wall ,x,y,o,msg);
for(int i = 0 ; i <= 1800 ;i = i + 1)
{
move_servo(i * 1);
int x = ir_distance();
printf("%d ", x);
}
printf("\n");
move_servo(0);
}
}
}
int TickFct_S(int state) {
static unsigned short servo_high; // pwm high time
static unsigned char cur_angle; // steering angle nibble
static unsigned char last; // steering var
// State Transitions
switch(state) {
case START_S:
state = INIT_S; break; // transition to INIT_S state
case INIT_S:
cur_angle = 0x30; // 'zero' out steering
if(ready) {
servo_on();
state = UPDATE_S;
}
break;
case WAIT_S:
if(!ready) {
// servo_off();
}
else {
// servo_on();
// if(!wired)
cur_angle = (incoming_data & 0x70) >> 4;
// else
// cur_angle = wired & 0x70;
state = UPDATE_S;
}
break;
case UPDATE_S:
state = WAIT_S; break;
default:
state = START_S; // error likely happened, restart
break;
}
// State Actions
switch(state) {
case UPDATE_S:
switch(cur_angle) {
case 0:
servo_high = (MIDDLE + (STEP_RIGHT*3));
break;
case 1:
servo_high = (MIDDLE + (STEP_RIGHT*2));
break;
case 2:
servo_high = (MIDDLE + STEP_RIGHT);
break;
case 3:
servo_high = MIDDLE;
break;
case 4:
servo_high = (MIDDLE - (STEP_LEFT));
break;
case 5:
servo_high = (MIDDLE - (STEP_LEFT*2));
break;
case 6:
servo_high = (MIDDLE - (STEP_LEFT*3));
break;
default:
servo_high = MIDDLE;
break;
}
// move_servo(FREQ,servo_high); // comment out if using block below!
// following block allows servo to 'relax' after receiving direction
// new angle is different from previous angle
if(last != cur_angle) {
last = cur_angle;
move_servo(FREQ,servo_high);
}
// same angle, go limp
else {
move_servo(0,0);
}
break;
default:
break;
}
return state;
}
/**
* Takes 180 data points from the ping sensor, converts all values into cm, and puts all objects into the given container
* @param objects an array of objects used to store any data collected by sweepScan
*/
void sweepScan(Object objects[]) {
int degrees = 0;
int objectCount = 0; //number of objects already scanned
int prevDetectionStatus = 0; //previous state of object detection
move_servo(0); //move servo to starting position
wait_ms(1000); //wait for initializations to finish
int finalValuesCalculated = 1;
while(degrees < 180) { //for one full rotation
move_servo(degrees); //sweep servo
degrees++; //increments of 2 degrees
ping_read(delta); //take ping sensor data
wait_ms(10); //wait for return pulse
int foundDelta = delta;
pingDistance = timeToDist(foundDelta); //convert ping data to cm
quantization = avgSensorResults(); //read from ADC channel 2 (IR sensor)
IRdistance = 2364.5*(pow(quantization, -0.888)); //convert quantization to distance in cm
int objectDetected = 0; //whether or not an object is being detected
//int distDifference = abs(IRdistance-pingDistance); //determine the absolute value of the difference between sensor values
if (IRdistance < 85 && prevDetectionStatus == 0) { //if an object is near and was not previously being detected
objectDetected = 1; //an object is near
Object scannedObject; //create a new object struct
scannedObject.degreePosition = degrees; //set degrees to current servo position
scannedObject.scannedDegrees = 1; //currently been scanned for one degree
scannedObject.cmDistance = 0; //distance according to ping sensor
scannedObject.isValid = 1;
objects[objectCount] = scannedObject; //add object to objects array
objectCount++; //increment object count by 1
prevDetectionStatus = objectDetected;
finalValuesCalculated = 0;
}
else if (IRdistance < 150 && prevDetectionStatus == 1) { //if an object is still being detected
objectDetected = 1;
}
else if (IRdistance > 150 && prevDetectionStatus == 1) { //if a large change in IRdistance has occurred (noise) but an object was being scanned
objectDetected = 1; //ignore IRdistance and continue scanning object
prevDetectionStatus = 0; //if the large gap persists, assume object is no longer being scanned
}
if (objectDetected) { //if currently scanning an object
objects[objectCount-1].scannedDegrees++; //increase number of degrees scanned for each servo rotation
objects[objectCount-1].cmDistance += pingDistance;
}
if (objectDetected == 0 && finalValuesCalculated == 0) { //if the object is no longer being detected, perform final calculations
objects[objectCount-1].cmDistance = (objects[objectCount-1].cmDistance/objects[objectCount-1].scannedDegrees);
objects[objectCount-1].cmWidth = ((2*objects[objectCount-1].cmDistance) * tan(((objects[objectCount-1].scannedDegrees)*3.14)/360)); //calculate width using angular diameter formula
finalValuesCalculated = 1;
}
//lprintf("Objects: %d\nDegrees: %d\nWidth: %d", objectCount, objects[objectCount-1].scannedDegrees, objects[objectCount-1].cmWidth); //FOR DEBUG ONLY
/*char toPrint[31]; //contains string to pass to putty
toPrint[0] = ' ';
sprintf(toPrint, "%d %d %lu %d\n\r", degrees, IRdistance, pingDistance, objectDetected);
serial_putString(toPrint, 29); //send string to putty for debug
*/
}
}
/**
* Takes 180 data measurements from the ping sensor, converts them to cm values, and determines the smallest object's index, width, and distance.
*
*/
void scanSmallestObj() {
int degrees = 0;
Object objects[10]; //holds scanned objects for later analysis
int objectCount = 0; //number of objects already scanned
int prevDetectionStatus = 0; //previous state of object detection
move_servo(0); //move servo to starting position
wait_ms(1500); //wait for initializations to finish
int finalValuesCalculated = 1;
while(degrees < 180) { //for one full rotation
move_servo(degrees); //sweep servo
wait_ms(10);
degrees++; //increments of 2 degrees
ping_read(delta); //take ping sensor data
wait_ms(50); //wait for return pulse
int foundDelta = delta;
pingDistance = timeToDist(foundDelta); //convert ping data to cm
quantization = avgSensorResults(); //read from ADC channel 2 (IR sensor)
IRdistance = 2364.5*(pow(quantization, -0.888)); //convert quantization to distance in cm
int objectDetected = 0; //whether or not an object is being detected
//int distDifference = abs(IRdistance-pingDistance); //determine the absolute value of the difference between sensor values
if (IRdistance < 85 && prevDetectionStatus == 0) { //if an object is near and was not previously being detected
objectDetected = 1; //an object is near
Object scannedObject; //create a new object struct
scannedObject.degreePosition = degrees; //set degrees to current servo position
scannedObject.scannedDegrees = 1; //currently been scanned for one degree
scannedObject.cmDistance = 0; //distance according to ping sensor
objects[objectCount] = scannedObject; //add object to objects array
objectCount++; //increment object count by 1
prevDetectionStatus = objectDetected;
finalValuesCalculated = 0;
}
else if (IRdistance < 150 && prevDetectionStatus == 1) { //if an object is still being detected
objectDetected = 1;
}
else if (IRdistance > 150 && prevDetectionStatus == 1) { //if a large change in IRdistance has occurred (noise) but an object was being scanned
objectDetected = 1; //ignore IRdistance and continue scanning object
prevDetectionStatus = 0; //if the large gap persists, assume object is no longer being scanned
}
if (objectDetected) { //if currently scanning an object
objects[objectCount-1].scannedDegrees++; //increase number of degrees scanned for each servo rotation
objects[objectCount-1].cmDistance += pingDistance;
}
if (objectDetected == 0 && finalValuesCalculated == 0) { //if the object is no longer being detected, perform final calculations
objects[objectCount-1].cmDistance = (objects[objectCount-1].cmDistance/objects[objectCount-1].scannedDegrees);
objects[objectCount-1].cmWidth = ((2*objects[objectCount-1].cmDistance) * tan(((objects[objectCount-1].scannedDegrees)*3.14)/360)); //calculate width using angular diameter formula
finalValuesCalculated = 1;
}
lprintf("Objects: %d\nDegrees: %d\nWidth: %d", objectCount, objects[objectCount-1].scannedDegrees, objects[objectCount-1].cmWidth); //FOR DEBUG ONLY
char toPrint[31]; //contains string to pass to putty
toPrint[0] = ' ';
sprintf(toPrint, "%d %d %lu %d\n\r", degrees, IRdistance, pingDistance, objectDetected);
serial_putString(toPrint, 29); //send string to putty
}
int smallestWidth = 1023; //used to determine smallest object
int index = 0; //current object index
int removedObjects = 0; //running count of objects thrown out due to size or distance
int prevRemovedObjects = 0; //objects removed prior to indicated index
int totalRemovedObjects = 0;
int loopRuns = 0;
for (int i = 0; i < objectCount; i++) {
if (objects[i].cmWidth <= 3 || objects[i].cmDistance > 100) { //if the object is very small or very far away, throw it out
removedObjects++;
loopRuns++;
totalRemovedObjects++;
}
else if ((objects[i].cmWidth < smallestWidth) && objects[i].cmWidth > 3) { //check if current object has new smallest width
smallestWidth = objects[i].cmWidth; //replace smallest width with new value
index = i; //lock onto index
prevRemovedObjects = removedObjects; //use for index compensation before final print statement
removedObjects = 0; //reset value for previous removed objects
}
}
lprintf("Index: %d of %d\nDist (cm): %d\nAngular width: %d\nWidth (cm): %d\n", (index-prevRemovedObjects+1), (objectCount-removedObjects+prevRemovedObjects), objects[index].cmDistance, objects[index].scannedDegrees, objects[index].cmWidth); //final results
move_servo(objects[index].degreePosition); //point to smallest object
}
// update mount position - should be called periodically
void AP_Mount_Servo::update()
{
static bool mount_open = 0; // 0 is closed
// check servo map every three seconds to allow users to modify parameters
uint32_t now = hal.scheduler->millis();
if (now - _last_check_servo_map_ms > 3000) {
check_servo_map();
_last_check_servo_map_ms = now;
}
switch(get_mode()) {
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
{
_angle_bf_output_deg = _state._retract_angles.get();
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
_angle_bf_output_deg = _state._neutral_angles.get();
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
// earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
stabilize();
break;
}
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
// update targets using pilot's rc inputs
update_targets_from_rc();
stabilize();
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control);
stabilize();
}
break;
}
default:
//do nothing
break;
}
// move mount to a "retracted position" into the fuselage with a fourth servo
bool mount_open_new = (get_mode() == MAV_MOUNT_MODE_RETRACT) ? 0 : 1;
if (mount_open != mount_open_new) {
mount_open = mount_open_new;
move_servo(_open_idx, mount_open_new, 0, 1);
}
// write the results to the servos
move_servo(_roll_idx, _angle_bf_output_deg.x*10, _state._roll_angle_min*0.1f, _state._roll_angle_max*0.1f);
move_servo(_tilt_idx, _angle_bf_output_deg.y*10, _state._tilt_angle_min*0.1f, _state._tilt_angle_max*0.1f);
move_servo(_pan_idx, _angle_bf_output_deg.z*10, _state._pan_angle_min*0.1f, _state._pan_angle_max*0.1f);
}
bool Servo2D::updateLoop(float fDelta) {
glm::vec2 pos = redrawLaser();
move_servo(servo_, pos.x, pos.y);
heartbeat_servo(servo_);
return true;
}
/**
* Used to control the robot.
* Receive and transmit data, measure the distance from object and navigate to the retrieval zone.
**/
int main(void)
{
lcd_init();
timer3_init();
timer_init();
ADC_init();
USART_Init();
oi_t *sensor_data = oi_alloc();
oi_init(sensor_data);//should turn the iRobot Create's power LED yellow
lcd_init();
serial_puts("Start");
//USART_Transmit(13);
//USART_Transmit(10);
int TempAngle[4] = {0,0,0,0};
int TempIR[4] = {0,0,0,0};
int pos[4] = {0,0,0,0};
int AddIR[4] = {0,0,0,0};
int count[4] = {0,0,0,0};
int found = 0;
int x1 = 0;
int x2 = 0;
int x3 = 0;
int x4 = 0;
unsigned angle = 0;
unsigned char IR = 0;
volatile int i=0;
volatile int x = 0;
char command;
char display[100];
char display1[20];
char display2[20];
char display3[20];
char display4[20];
char display5[100];
char display6[100];
while (1)
{
command = USART_Recieve();
USART_Transmit(command);
//USART_Transmit(13);
//USART_Transmit(10);
if (command == '1')
{
found = 0;
angle = 0;
int t;
int TempAngle[6] = {0, 0,0,0,0,0};
int TempIR[6] = {0, 0,0,0,0,0};
int pos[6] = {0, 0,0,0,0,0};
int AddIR[6] = {0, 0, 0,0,0,0};
int count[6] = {0, 0, 0,0,0,0};
for (angle = 0;angle < 181;angle++)
{
move_servo(angle);
wait_ms(20);
IR = 0;
IR = 42800*pow(ADC_read(2),-1.23);
sprintf(display6, "Angle: %5d IR: %5d",angle,IR);
serial_puts(display6);
if (IR < 80)
{
TempAngle[found]++;
count[found]++;
AddIR[found]+=IR;
TempIR[found]=AddIR[found]/count[found];
}
else
{
if(TempAngle[found] < 5)
{
TempAngle[found] = 0;
}
else
{
pos[found] = angle- TempAngle[found]/2;
if (TempIR[found]*TempAngle[found]< 460)
{
USART_Transmit(13);
USART_Transmit(10);
for (int i = 0;i<strlen(s8);i++)
{
USART_Transmit(s8[i]);
}
sprintf(display5, "object position: %5d",pos[found]);
serial_puts(display5);
}
sprintf(display, "object position: %5d IR: %5d object size: %5d",pos[found],TempIR[found],TempAngle[found]);
serial_puts(display);
USART_Transmit(13);
USART_Transmit(10);
found++;
}
}
}
OCR3B = 1000-1; //return to 0 degree
}
if (command == 'w')
{
move_forward(sensor_data,20);
}
if (command == 's')
{
move_backforward(sensor_data,20);
}
if (command == 'a')
{
turn_clockwise(sensor_data,82);
}
if (command == 'd')
{
turn_counterclockwise(sensor_data,82);
}
if (command == 'q')
{
turn_clockwise(sensor_data,38);
}
if (command == 'e')
{
turn_counterclockwise(sensor_data, 38);
}
if (command == '8')
{
move_forward(sensor_data,5);
}
if (command == '5')
{
move_backforward(sensor_data,5);
}
if (command == 'p')
{
oi_t* sensor = oi_alloc();
oi_init(sensor);
load_songs();
oi_play_song(songings);
}
if(command == 'k')
{
oi_update(sensor_data);
x1 = sensor_data->cliff_left_signal;
x2 = sensor_data->cliff_right_signal;
x3 = sensor_data->cliff_frontleft_signal;
x4 = sensor_data->cliff_frontright_signal;
sprintf (display1, "left = %d",x1);
sprintf (display2, "right = %d",x2);
sprintf (display3, "front left = %d",x3);
sprintf (display4, "front right = %d",x4);
USART_Transmit(13);
USART_Transmit(10);
serial_puts(display1);
serial_puts(display3);
serial_puts(display4);
serial_puts(display2);
if (x1>500||x2>500||x3>500||x4>500)
{
USART_Transmit(13);
USART_Transmit(10);
for (int i=0;i<strlen(s6);i++)
{
USART_Transmit(s6[i]);
}
}
}
}
}
