273 lines
9.9 KiB
C#
273 lines
9.9 KiB
C#
/*- AdvancedServo simple -
|
|
0****************************************************************************************
|
|
* This example simply displays the Phidget AdvancedServo serial number when it is
|
|
* attached and sets the servo positions, velocity, and acceleration to 0. Then we will
|
|
* do a simple simulation of a basic movement of a servo motor at 100.00 velocity and
|
|
* 100.00 Acceleration. I decided to leave out the current change event handler for
|
|
* easier readability. For a more detailed example, see the Servo-full example.
|
|
*
|
|
* Please note that this example was designed to work with only one Phidget AdvanceServo
|
|
* connected.
|
|
* For an example showing how to use two Phidgets of the same time concurrently, please see the
|
|
* Servo-multi example in the Servo Examples.
|
|
|
|
* Copyright 2007 Phidgets Inc. All rights reserved.*/
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
//Needed for the AdvancedServo class, phidget base classes, and PhidgetException class
|
|
using Phidgets;
|
|
//Needed for the event handling classes
|
|
using Phidgets.Events;
|
|
//Using this simply for the sleep() method so that the for loop will wail for the motor
|
|
//to finish moving to the previous new position before setting a new position
|
|
using System.Threading;
|
|
|
|
using System.IO.Ports;
|
|
|
|
namespace AdvancedServo_simple {
|
|
class Program {
|
|
private static AdvancedServo advServo;
|
|
private static SerialPort led;
|
|
static void Main(string[] args) {
|
|
connectLed("COM50");
|
|
trainOn(1);
|
|
Console.WriteLine("Train Engaged. Press Key");
|
|
Console.ReadLine();
|
|
trainMove();
|
|
readStop();
|
|
//Console.WriteLine("Wait to its stop. Press Key, then it Laser and Move");
|
|
//Console.ReadLine();
|
|
controlLaser(169861);
|
|
trainMove();
|
|
readStop();
|
|
//Console.WriteLine("Wait to its stop. Press Key, then it Paint and Move");
|
|
//Console.ReadLine();
|
|
controlPaint(169889);
|
|
trainMove();
|
|
readStop();
|
|
//Console.WriteLine("Wait to its stop. Press Key, then it put the Glas and Move");
|
|
//Console.ReadLine();
|
|
controlGlass(169887);
|
|
trainMove();
|
|
readStop();
|
|
//Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
|
|
//Console.ReadLine();
|
|
changeDir(0);
|
|
trainMove();
|
|
readStop();
|
|
//Console.WriteLine("Wait to its stop. Press Key, then it moves Back");
|
|
//Console.ReadLine();
|
|
trainMove();
|
|
led.Close();
|
|
}
|
|
|
|
private static void readStop() {
|
|
string r = led.ReadLine();
|
|
while(!System.Text.RegularExpressions.Regex.Match(r, "\\*r[0-9]=[0-9A-Fa-f]*\\$.*#").Success) {
|
|
r = led.ReadLine();
|
|
}
|
|
}
|
|
|
|
private static void changeDir(byte dir) {
|
|
serialSend('d', 2, dir); //Richtung = 0
|
|
}
|
|
|
|
private static void trainMove() {
|
|
serialSend('b', 2, 0); //Bremse aus
|
|
}
|
|
|
|
private static void trainOn(byte dir) {
|
|
serialSend('p', 2, 1); //Speed = 1
|
|
serialSend('d', 2, dir); //Richtung = 1
|
|
serialSend('b', 2, 1); //Bremse = an
|
|
}
|
|
|
|
private static void setLaser(byte p) {
|
|
serialSend('l', 1, p);
|
|
}
|
|
|
|
private static void setColor(byte p) {
|
|
serialSend('c', 1, p);
|
|
}
|
|
|
|
private static int serialReadCurrent() {
|
|
serialSend('u', 1, 0, false);
|
|
string r = led.ReadLine();
|
|
return Int32.Parse(r.Substring(3, (r.Length - 7)));
|
|
}
|
|
|
|
private static void serialSend(char key, byte pos, byte val, bool loud = true) {
|
|
string cdata = key + pos + "=" + val;
|
|
char c = '$';
|
|
foreach(char t in cdata) {
|
|
c = (char)(c ^ t);
|
|
}
|
|
string s = "*"+key+pos.ToString()+"="+val.ToString()+"$"+c+"#";
|
|
if(loud) {
|
|
Console.WriteLine("Serial: " + s);
|
|
}
|
|
led.WriteLine(s);
|
|
}
|
|
|
|
private static void connectLed(string led_com) {
|
|
led = new SerialPort(led_com, 57600);
|
|
led.Open();
|
|
led.WriteLine("");
|
|
}
|
|
|
|
private static void controlLaser(int serial) {
|
|
connectServo(serial);
|
|
if(!advServo.Attached) { return; }
|
|
|
|
setStartPosition(new int[] { 0, 77, 175, 155, 84 });
|
|
Thread.Sleep(200);
|
|
Console.WriteLine("Servos are in Start!");
|
|
|
|
setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
|
|
setPosition(new int[] { -1, 83, 97, 116, 44 }, 200);
|
|
setLaser(1);
|
|
setPosition(new int[] { -1, 102, 90, 108, 42 }, 20);
|
|
setPosition(new int[] { -1, 98, 79, 90, 48 }, 20);
|
|
setPosition(new int[] { -1, 83, 82, 95, -1 }, 20);
|
|
setPosition(new int[] { -1, 83, 97, 116, 44 }, 20);
|
|
setLaser(0);
|
|
setPosition(new int[] { -1, 77, 157, 134, 84 }, 400);
|
|
setPosition(new int[] { -1, 77, 175, 155, 84 }, 400);
|
|
//Thread.Sleep(100);
|
|
shutdownServo();
|
|
}
|
|
|
|
private static void controlPaint(int serial) {
|
|
connectServo(serial);
|
|
if(!advServo.Attached) { return; }
|
|
/* not needed, turn, */
|
|
setStartPosition(new int[] { 0, 87, 175, 160, 92 });
|
|
Thread.Sleep(1000);
|
|
Console.WriteLine("Servos are in Start!");
|
|
|
|
setPosition(new int[] { -1, 87, 153, 138, 92 }, 400);
|
|
setPosition(new int[] { -1, 44, 111, 161, 63 }, 200);
|
|
setColor(2);
|
|
setPosition(new int[] { -1, 90, -1, -1, -1 }, 50);
|
|
setColor(0);
|
|
setPosition(new int[] { -1, -1, 140, -1, -1 }, 200);
|
|
setPosition(new int[] { -1, -1, 71, 55, 110 }, 200);
|
|
setPosition(new int[] { -1, -1, 43, 42, 175 }, 200);
|
|
setColor(2);
|
|
setPosition(new int[] { -1, 60, 33, 22, -1 }, 50);
|
|
setColor(0);
|
|
setPosition(new int[] { -1, -1, 63, 45, 150 }, 200);
|
|
setPosition(new int[] { -1, 50, 68, 76, 132 }, 200);
|
|
setColor(7);
|
|
setPosition(new int[] { -1, -1, -1, -1, 160 }, 100);
|
|
setPosition(new int[] { -1, 60, -1, -1, 172 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, -1, 144 }, 100);
|
|
setPosition(new int[] { -1, 70, -1, -1, 150 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
|
|
setPosition(new int[] { -1, 80, -1, -1, -1 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, -1, 155 }, 100);
|
|
setPosition(new int[] { -1, 90, 74, 84, 148 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, -1, 175 }, 100);
|
|
setPosition(new int[] { -1, 100, -1, -1, 166 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, -1, 146 }, 100);
|
|
setColor(0);
|
|
setPosition(new int[] { -1, -1, 100, -1, -1 }, 200);
|
|
setPosition(new int[] { 0, 87, 175, 160, 92 }, 400);
|
|
//Thread.Sleep(100);
|
|
shutdownServo();
|
|
}
|
|
|
|
private static void controlGlass(int serial) {
|
|
connectServo(serial);
|
|
if(!advServo.Attached) { return; }
|
|
|
|
setStartPosition(new int[] { 20, 68, 175, 175, 20 });
|
|
Thread.Sleep(1000);
|
|
Console.WriteLine("Servos are in Start!");
|
|
|
|
setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
|
|
setPosition(new int[] { -1, 154, 88, 88, -1 }, 400);
|
|
setPosition(new int[] { -1, -1, 86, 116, 42 }, 200);
|
|
setPosition(new int[] { -1, -1, 77, -1, 48 }, 50);
|
|
setPosition(new int[] { 160, -1, -1, -1, -1 }, 200);
|
|
setPosition(new int[] { -1, -1, 85, -1, -1 }, 20);
|
|
setPosition(new int[] { -1, -1, -1, 106, -1 }, 200);
|
|
setPosition(new int[] { -1, -1, 118, 122, 174 }, 200);
|
|
Console.WriteLine("Picked Up Glas");
|
|
setPosition(new int[] { -1, 13, -1, -1, -1 }, 300);
|
|
setPosition(new int[] { -1, -1, 90, 100, 20 }, 200);
|
|
|
|
setPosition(new int[] { -1, -1, 82, 97, 25 }, 20); // Fine Settings
|
|
Console.WriteLine("Placed Glas!");
|
|
Thread.Sleep(1000);
|
|
setPosition(new int[] { -1, -1, 90, 90, 20 }, 20); // Fine Lift
|
|
|
|
setPosition(new int[] { -1, -1, 103, 105, 80 }, 150);
|
|
setPosition(new int[] { -1, -1, -1, -1, 174 }, 300);
|
|
setPosition(new int[] { -1, 154, 118, 122, -1 }, 300);
|
|
setPosition(new int[] { -1, -1, 86, 102, 42 }, 300);
|
|
setPosition(new int[] { -1, -1, 81, 113, 48 }, 100);
|
|
setPosition(new int[] { -1, -1, -1, 117, -1 }, 20);
|
|
setPosition(new int[] { 20, -1, 78, -1, -1 }, 150);
|
|
setPosition(new int[] { -1, -1, 81, 111, -1 }, 20);
|
|
setPosition(new int[] { -1, -1, 90, -1, -1 }, 100);
|
|
Console.WriteLine("Put Glas back!");
|
|
setPosition(new int[] { 20, 68, 160, 162, 20 }, 400);
|
|
setPosition(new int[] { 20, 68, 175, 175, 20 }, 400);
|
|
Thread.Sleep(100);
|
|
shutdownServo();
|
|
}
|
|
|
|
private static void shutdownServo() {
|
|
servosStop();
|
|
//Console.WriteLine("End. Press Key");
|
|
//Console.ReadLine();
|
|
return;
|
|
}
|
|
|
|
private static void connectServo(int serial) {
|
|
advServo = new AdvancedServo();
|
|
advServo.open(serial);
|
|
Console.WriteLine("Waiting for Advanced Servo \"" + serial + "\" to be attached...");
|
|
advServo.waitForAttachment();
|
|
//Console.WriteLine("Pess KEY... To start!");
|
|
//Console.ReadLine();
|
|
}
|
|
|
|
private static void setPosition(int[] pos, int speed) {
|
|
for(int i = 0; i < 5; i++) {
|
|
if(pos[i] != -1) {
|
|
AdvancedServoServo s = advServo.servos[i];
|
|
s.Acceleration = speed;
|
|
s.Position = pos[i];
|
|
}
|
|
}
|
|
for(int i = 0; i < 5; i++) {
|
|
if(pos[i] != -1) {
|
|
AdvancedServoServo s = advServo.servos[i];
|
|
while(Math.Abs(s.Position - pos[i]) > 0.01) { Thread.Sleep(10); s.Position = pos[i]; }
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void servosStop() {
|
|
for(int i = 0; i < 5; i++) {
|
|
advServo.servos[i].Engaged = false;
|
|
}
|
|
}
|
|
|
|
private static void setStartPosition(int[] positions) {
|
|
for(int i = 0; i < 5; i++) {
|
|
AdvancedServoServo s = advServo.servos[i];
|
|
s.Type = ServoServo.ServoType.TOWERPRO_MG90;
|
|
s.Acceleration = 10000;
|
|
s.VelocityLimit = 200;
|
|
s.Position = positions[i];
|
|
s.Engaged = true;
|
|
}
|
|
}
|
|
}
|
|
}
|