/*
* ======== 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);
}
