90 lines
2.5 KiB
C#
90 lines
2.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Text;
|
|
using Phidgets;
|
|
using System.Threading;
|
|
|
|
namespace ebbits.Robots {
|
|
public abstract class AbstractRobot {
|
|
protected AdvancedServo servo;
|
|
protected int cservos;
|
|
protected string name;
|
|
private Thread t;
|
|
protected void init(int id) {
|
|
servo = new AdvancedServo();
|
|
servo.open(id);
|
|
Logger.Info("Waiting for Servo \"" + id + "\" is attached");
|
|
servo.waitForAttachment();
|
|
if(!servo.Attached) { throw new AccessViolationException("Robot not Found"); }
|
|
}
|
|
public abstract void run();
|
|
protected void setPosition(int[] pos, int speed) {
|
|
for(int i = 0; i < cservos; i++) {
|
|
if(pos[i] != -1) {
|
|
AdvancedServoServo s = servo.servos[i+ 5 - cservos];
|
|
s.Acceleration = speed;
|
|
s.Position = pos[i];
|
|
}
|
|
}
|
|
|
|
bool waiting = true;
|
|
while(waiting) {
|
|
waiting = false;
|
|
for(int i = 0; i < cservos; i++) {
|
|
if(pos[i] != -1) {
|
|
AdvancedServoServo s = servo.servos[i + 5 - cservos];
|
|
s.Position = pos[i];
|
|
try {
|
|
if(Math.Abs(s.Position - pos[i]) > 0.01) {
|
|
waiting = true;
|
|
}
|
|
} catch(PhidgetException) {
|
|
Logger.Warn("Exeption!");
|
|
s.Position = pos[i];
|
|
}
|
|
Thread.Sleep(10);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
protected void setStartPosition(int[] positions, int sleep) {
|
|
for(int i = 0; i < cservos; i++) {
|
|
AdvancedServoServo s = servo.servos[i + 5 - cservos];
|
|
s.Type = ServoServo.ServoType.TOWERPRO_MG90;
|
|
s.Acceleration = 10000;
|
|
s.VelocityLimit = 200;
|
|
s.Position = positions[i];
|
|
s.Engaged = true;
|
|
}
|
|
Thread.Sleep(sleep);
|
|
}
|
|
protected void shutdownServo() {
|
|
for(int i = 0; i < cservos; i++) {
|
|
servo.servos[i + 5 - cservos].Engaged = false;
|
|
}
|
|
if(t.IsAlive) {
|
|
t.Abort();
|
|
}
|
|
}
|
|
|
|
protected void startPowerMess() {
|
|
this.t = new System.Threading.Thread(current_runnter);
|
|
t.Start();
|
|
}
|
|
|
|
private void current_runnter(object obj) {
|
|
DateTime t = DateTime.Now;
|
|
double current = 0;
|
|
while(true) {
|
|
double c = 0;
|
|
for(int i = 0; i < 5; i++) {
|
|
c += this.servo.servos[0].Current;
|
|
}
|
|
current += (c * 12 * (DateTime.Now.Ticks - t.Ticks)) / 1000000;
|
|
t = DateTime.Now;
|
|
Logger.Info(this.name+" Watt: " + current.ToString());
|
|
System.Threading.Thread.Sleep(100);
|
|
}
|
|
}
|
|
}
|
|
} |