/* * ======== main ======== * Create a task. * Synchronize all processors. * Register an event with Notify. */ Int main(Int argc, Char* argv[]) { selfId = MultiProc_self(); System_printf("main: MultiProc id = %d\n", selfId); System_printf("main: MultiProc name = %s\n", MultiProc_getName(selfId)); if (numCores == 0) { numCores = MultiProc_getNumProcessors(); } /* * Determine which processors Notify will communicate with based on the * local MultiProc id. */ srcProc = ((selfId - 1 + numCores) % numCores); dstProc = ((selfId + 1) % numCores); attachAll(numCores); BIOS_start(); return (0); }
void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle) { attachAll(); if(servo_left_angle < 10) servo_left_angle = 10; if(servo_left_angle > 120) servo_left_angle = 120; if(servo_right_angle < 10) servo_right_angle = 10; if(servo_right_angle > 110) servo_right_angle = 110; if(servo_left_angle + servo_right_angle > 160) { servo_right_angle = 160 - servo_left_angle; return; } writeServoAngle(SERVO_ROT_NUM,servo_rot_angle,true); writeServoAngle(SERVO_LEFT_NUM,servo_left_angle,true); writeServoAngle(SERVO_RIGHT_NUM,servo_right_angle,true); writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true); // refresh logical servo angle cache cur_rot = servo_rot_angle; cur_left = servo_left_angle; cur_right = servo_right_angle; cur_hand = servo_hand_rot_angle; }
/* * ======== main ======== */ Int main(Int argc, Char* argv[]) { selfId = MultiProc_self(); System_printf("Core (\"%s\") starting\n", MultiProc_getName(selfId)); if (numCores == 0) { numCores = MultiProc_getNumProcessors(); } attachAll(numCores); System_sprintf(localQueueName, "CORE%d", selfId); System_sprintf(nextQueueName, "CORE%d", ((selfId + 1) % numCores)); System_sprintf(prevQueueName, "CORE%d", (selfId - 1 + numCores) % numCores); /* Create a message queue. */ messageQ = MessageQ_create(localQueueName, NULL); if (messageQ == NULL) { System_abort("MessageQ_create failed\n" ); } BIOS_start(); return (0); }
/****************************************************************************** * MAIN FUNCTION *****************************************************************************/ Int main(Int argc, Char* argv[]){ srand(time(NULL)); selfId = CSL_chipReadReg(CSL_CHIP_DNUM); if (numCores == 0){ numCores = MultiProc_getNumProcessors(); } /* Attach All Cores */ attachAll(numCores); /* Create a MessageQ */ System_sprintf(localQueueName, "%s", MultiProc_getName(MultiProc_self())); messageQ = MessageQ_create(localQueueName, NULL); if (messageQ == NULL){ System_abort("MessageQ_create failed\n"); } BIOS_start(); return (0); }
void uArmClass::moveToOpts(double x, double y, double z, double hand_angle, byte relative_flags, double time, byte path_type, byte ease_type) { attachAll(); // find current position using cached servo values double current_x; double current_y; double current_z; getCalXYZ(cur_rot, cur_left, cur_right, current_x, current_y, current_z); // deal with relative xyz positioning byte posn_relative = (relative_flags & F_POSN_RELATIVE) ? 1 : 0; x = current_x * posn_relative + x; y = current_y * posn_relative + y; z = current_z * posn_relative + z; // find target angles double tgt_rot; double tgt_left; double tgt_right; calAngles(x, y, z, tgt_rot, tgt_left, tgt_right); // deal with relative hand orientation if (relative_flags & F_HAND_RELATIVE) { hand_angle += cur_hand; // rotates a relative amount, ignoring base rotation } else if (relative_flags & F_HAND_ROT_REL) { hand_angle = hand_angle + cur_hand + (tgt_rot - cur_rot); // rotates relative to base servo, 0 value keeps an object aligned through movement } if (time > 0) { if (path_type == PATH_ANGLES) { // we will calculate angle value targets double rot_array[INTERP_INTVLS]; double left_array[INTERP_INTVLS]; double right_array[INTERP_INTVLS]; double hand_array[INTERP_INTVLS]; interpolate(cur_rot, tgt_rot, rot_array, ease_type); interpolate(cur_left, tgt_left, left_array, ease_type); interpolate(cur_right, tgt_right, right_array, ease_type); interpolate(cur_hand, hand_angle, hand_array, ease_type); for (byte i = 0; i < INTERP_INTVLS; i++) { writeAngle(rot_array[i], left_array[i], right_array[i], hand_array[i]); //writeServoAngle(SERVO_ROT_NUM, rot_array[i], true); // writeServoAngle(SERVO_LEFT_NUM, left_array[i],true); // writeServoAngle(SERVO_RIGHT_NUM, right_array[i],true); //writeLeftRightServoAngle(left_array[i], right_array[i], true); //writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i],true); delay(time * 1000 / INTERP_INTVLS); } } else if (path_type == PATH_LINEAR) { // we will calculate linear path targets double x_array[INTERP_INTVLS]; double y_array[INTERP_INTVLS]; double z_array[INTERP_INTVLS]; double hand_array[INTERP_INTVLS]; interpolate(current_x, x, x_array, ease_type); interpolate(current_y, y, y_array, ease_type); interpolate(current_z, z, z_array, ease_type); interpolate(cur_hand, hand_angle, hand_array, ease_type); for (byte i = 0; i < INTERP_INTVLS; i++) { // double rot; double rot,left, right; calAngles(x_array[i], y_array[i], z_array[i], rot, left, right); // Serial.println(rot); // Serial.println(left); // Serial.println(right); // Serial.println(); //writeServoAngle(SERVO_ROT_NUM, rot,true); // writeServoAngle(SERVO_LEFT_NUM, tgt_left,true); // writeServoAngle(SERVO_RIGHT_NUM, tgt_right,true); //writeLeftRightServoAngle(left, right, true); // if(enable_hand) //writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true); writeAngle(rot, left, right, hand_array[i]); delay(time * 1000 / INTERP_INTVLS); } } } // set final target position at end of interpolation or "atOnce" //writeServoAngle(SERVO_ROT_NUM, tgt_rot, true); // writeServoAngle(SERVO_LEFT_NUM, tgt_left, true); // writeServoAngle(SERVO_RIGHT_NUM, tgt_right, true); //writeLeftRightServoAngle(tgt_left, tgt_right, true); //writeServoAngle(SERVO_HAND_ROT_NUM, hand_angle, true); writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle); }