Implementation of spiral motion based on spiral queue logic
The inverse method of the helical queue algorithm, which controls the motion of the two-axis motor according to the helical trajectory, as.
1. Spiral Queue Algorithm Analysis
is the spiral queue. Set 1 coordinates is (0,0), the x direction to the right is positive, the y direction downward is positive, for example, 7 coordinates is ( -1,-1), 2 coordinates is (1,0). Programming implementation input any point coordinates (x, y), the output of the corresponding number! (Transferred from the network)
Maximum max= (2*c+1) (2*c+1) per lap, and C is the number of laps from inside out.
The difference between these datum values and Max is 1C (top), 3C (left), 5C (bottom), 7C (right) (c for the current number of laps), and the Y coordinate (or equal to) the number of laps (i.e., c=y) on the top and bottom, and the x-coordinate (or equal to) the number of laps (that is, So the difference mentioned above can be represented as 1y,3x,5y,7x by coordinates.
Code implementation:
private static Object Spiral (int x, int y) { int c = MAX (ABS (x), ABS (y));//current coordinate is circled int max = (c * 2 + 1) * (c * 2 + 1);//current lap maximum if (y = =-c) {//top return max + (x + y); } else if (x = =-C) {//left return Max + (3 * x-y); } else if (y = = c) {//Bottom return max + (-x-5 * y); } else {//Right return max + ( -7 * x + y);} }
2. Spiral Motion
First, customize the coordinate operation to represent the logical location of the PD.
struct coordinate {public int X; public int Y; public coordinate (int a, int b) {X = A; Y = b; } public static bool operator = = (coordinate loc1, coordinate loc2) {return (Loc1. X = = Loc2. X) && (Loc1. Y = = Loc2. Y); } public static bool operator! = (coordinate loc1, coordinate loc2) {return! Loc1 = = LOC2); } public override bool Equals (Object loc) {return this = = (coordinate) loc; } public override int GetHashCode () {return base. GetHashCode (); } public static coordinate operator-(coordinate loc1, coordinate loc2) {return new coordinate (l Oc1. X-loc2. X, Loc1. Y-loc2. Y); } public static coordinate operator + (coordinate loc1, coordinate loc2) {return new coordinate (l Oc1. X + loc2. X, Loc1. Y + loc2. Y); } public override StrinG ToString () {return "(" + X + "," + Y + ")"; } }
Then the inverse method calculates the x, Y coordinates based on the number of steps.
public coordinate tolocation (int step, int pulse) {int c = (int) math.ceiling ((math.sqrt (step)-1)/2) ; int max = (int) Math.pow (2 * C + 1, 2); int x, y; y =-c;//top x =-(max + y-step); if (Math.Abs (x) <= Math.Abs (y)) {this.location.x = x * PULSE; THIS.LOCATION.Y = Y * PULSE; return this.location; } x =-c;//left y = max + 3 * X-STEP; if (Math.Abs (y) <= math.abs (x)) {this.location.x = x * PULSE; THIS.LOCATION.Y = Y * PULSE; return this.location; } y = c;//bottom x = max-5 * Y-STEP; if (Math.Abs (x) <= Math.Abs (y)) {this.location.x = x * PULSE; THIS.LOCATION.Y = Y * PULSE; return this.location; } x = C;//right y =-(max-7 * x-step); this.location.x = X * PULSE; THIS.LOCATION.Y = Y * PULSE; Locchange (); return this.location; }
Finally, the movement is realized according to the change of coordinates.
public void Start () {coordinate movetoloc, currentloc, Deltaloc; Currentloc = tolocation (1, 0); Loginfo = string. Format ("{0}: {1}{2}.", DateTime.Now.ToString ("HH:mm:ss"), "the Start location is", currentloc.tostring ()); Log. Savelogtotxt (Loginfo); Loginfo = string. Format ("{0}: {1}.", DateTime.Now.ToString ("HH:mm:ss"), "begin to move ... "); Log. Savelogtotxt (Loginfo); for (int step = 1; step <= this.romaxstep[0]; step++) {movetoloc = tolocation (step + 1, this . ropulse[0]); Deltaloc = Movetoloc-currentloc; Loginfo = string. Format ("{0}: step{1}{2}{3} ...", DateTime.Now.ToString ("HH:mm:ss"), Step + "", "Move to", movetoloc.tostring ()); Log. Savelogtotxt (Loginfo); BOOL MoveX = card. MoveX (deltaloc.x); BOOL Movey = card. Movey (DELTALOC.Y); if (MoveX = = False | | movey = = FALSE) Throw error return; Currentloc = Movetoloc; If RES > Rorestarget//break; } Loginfo = string. Format ("{0}: {1}.", DateTime.Now.ToString ("HH:mm:ss"), "Move Done"); Log. Savelogtotxt (Loginfo); Loginfo = string. Format ("{0}: {1}{2}.", DateTime.Now.ToString ("HH:mm:ss"), "The current location is", currentloc.tostring ()); Log. Savelogtotxt (Loginfo); }
The above is from 0 self-learning c#07--spiral Queue and spiral movement content, more relevant content please pay attention to topic.alibabacloud.com (www.php.cn)!