Mindboards

Code: Select all

#define MOTOR_POWER 60

int nSyncedTurnRatio;

/************************* Motion planning functions *****************************/

// void do_move(tMotor m, int d) - Command the motor m to move through the
// prescribed encoder counts d and block until the movement is complete.
void do_move(byte outputs int d)
{
   RotateMotorEx(outputs, MOTOR_POWER, d, nSyncedTurnRatio, true, true);
}

// void go_fwd (float dist_mm) - Move robot forward the given distance in mm. Blocks
// til movement done.
void go_fwd (float dist_mm)
{
   int dist = (dist_mm * MM_TO_ENC);
   nSyncedTurnRatio = 0;
   do_move(OUT_AC, dist);
}

// void turn_left (float ang_deg) - Turns robot left given angle in degrees. Blocks
// til movement done.
void turn_left (float ang_deg)
{
   int dist = (ang_deg * DEG_TO_ENC);
   if (dist > 0) {
      nSyncedTurnRatio = -100;
      do_move (OUT_AC, dist);
   }
}

// void turn_right (float ang_deg) - Turns robot right given angle in degrees. Blocks
// til movement done.
void turn_right (float ang_deg)
{
   int dist = (ang_deg * DEG_TO_ENC);
   if (dist > 0) {
      nSyncedTurnRatio = 100;
      do_move(OUT_AC, dist);
   }
}

Code: Select all

	bFloatDuringInactiveMotorPWM = false;
	nMotorPIDSpeedCtrl[left_m]  = mtrSpeedReg;
	nMotorPIDSpeedCtrl[right_m] = mtrSpeedReg;
	nPidUpdateInterval = 5;

Code: Select all

task drive()
{
	float dx, dy, dq_degrees;
	int i, j;

	bFloatDuringInactiveMotorPWM = false;
	nMotorPIDSpeedCtrl[left_m]  = mtrSpeedReg;
	nMotorPIDSpeedCtrl[right_m] = mtrSpeedReg;
	nPidUpdateInterval = 5;
	nMaxRegulatedSpeed = MAX_SPEED;

	do {
		for (i = 0; i < num_waypts; i++) {
			for (j = 0; j < num_segments; j++) {

				// Wait for most up to date values
				wait_for_filter = 1;
				while (wait_for_filter > 0) {
					wait1Msec(1);
				}

				dx = waypt_x[i] - avg_x;
				dy = waypt_y[i] - avg_y;
				dist = sqrt(dx * dx + dy * dy) / (float)(num_segments - j);
				ATAN2(dy, dx, dq);
				dq -= avg_q;
				limit_ang(dq);
				dq_degrees = dq / PI * 180.0;
				StringFormat(cmd, "Q:%3.0f D:%3.0f", dq_degrees, dist);
				if (dq > 0.0) {
					turn_left (dq_degrees);
				} else if (dq < 0.0) {
					turn_right (-dq_degrees);
				}
				go_fwd (dist);

				// Finished moving so OK to run filter again
				if (wait_for_drive > 0) {
					wait_for_drive--;
				}
			}
		}
	} while (cycle_waypoints);
}