void tgCPGCableControl::onStep(tgSpringCableActuator& subject, double dt)
{
assert(&subject == m_PID->getControllable());
m_controlTime += dt;
m_totalTime += dt;
if (m_controlTime >= m_controlStep)
{
if (usePID)
{
m_commandedTension = motorControl().control(*m_PID, dt, controlLength(), getCPGValue());
}
else
{
tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
m_commandedTension = motorControl().control(*basicAct, dt, controlLength(), getCPGValue());
}
m_controlTime = 0;
}
else
{
const double currentTension = subject.getTension();
if(usePID)
{
m_PID->control(dt, m_commandedTension, currentTension);
}
else
{
tgBasicActuator* basicAct = tgCast::cast<tgSpringCableActuator, tgBasicActuator>(&subject);
basicAct->moveMotors(dt);
}
}
}
//reset the position of all the motors
void resetMotors(unsigned int & serialPort)
{
#ifdef ENABLE_MOTORS
motorControl(serialPort, 'G', 0);
motorControl(serialPort, 'T', 0);
motorControl(serialPort, 'B', 0);
#endif
#ifdef ENABLE_STEERING
Move_Motor_Abs(STEERING_CENTER);
#endif
}
/*static*/
void controlUpdate(union sigval v)
{
#if 0 //用于示波器观测定时器
if(count)
{
count = 0;
gpioWrite(PIN_SIG, 1);
}
else
{
count = 1;
gpioWrite(PIN_SIG, 0);
}
#endif
#if 1
imuUpdate();
//printImuData();
pidControl();
motorControl();
joystickControl();
#endif
//control2();
}
int main()
{
int i;
init(0);
// connect camera to the screen
open_screen_stream();
// set all didgital outputs to +5V
for (i = 0; i < 8; i++)
{
// set all digital channels as outputs
select_IO(i,0);
write_digital(i,1);
}
while(1)
{
motorControl(line());
}
// terminate hardware
close_screen_stream();
set_motor(1,0);
set_motor(2,0);
return 0;
}
// controls motor PWM values based on current and target position using a proportional controller (triggered by interrupt)
// total duration = 439us, therefore max freq = 2kHz. We use 200Hz (5ms), where 0.5ms = motor control, 4.5ms = program runtime
void fingerPosCtrl(void)
{
static int fingerCounter = 0; // counts through attached _fingers
signed int motorSpeed = 0; // used to calculate the motor speed as a vector (±255)
float m; // the proportional gradient
signed int vectorise = 1; // changes the sign '±' of the value
int proportionalOffset = 150;
signed int motorStopOffset = 20;
if(_TotalFingerCount > 0) // if _fingers are attached
{
// count through each finger at each function call
if(fingerCounter < (_TotalFingerCount - 1))
fingerCounter++;
else
fingerCounter = 0;
// read position
_fingers[fingerCounter].CurrPos = analogRead(_fingers[fingerCounter].Pin.sns); // 424us
// invert finger direction if enabled
if(_fingers[fingerCounter].invert)
_fingers[fingerCounter].CurrPos = 1023 - _fingers[fingerCounter].CurrPos;
// calc positional error
_fingers[fingerCounter].CurrErr = (signed int) (_fingers[fingerCounter].TargPos - _fingers[fingerCounter].CurrPos);
// speed/position line gradient
m = (float) (((float) _fingers[fingerCounter].TargSpeed)/((float) proportionalOffset));
// change the ± sign on the motorSpeed depending on required direction
if(_fingers[fingerCounter].CurrErr>=0)
vectorise = -1;
// constrain speed
if(abs(_fingers[fingerCounter].CurrErr) < motorStopOffset)
motorSpeed = 0; // motor dead zone
else if(_fingers[fingerCounter].CurrErr > (signed int)(proportionalOffset + motorStopOffset))
motorSpeed = _fingers[fingerCounter].TargSpeed; // set to max speed speed depending on direction
else if(_fingers[fingerCounter].CurrErr < -(signed int)(proportionalOffset + motorStopOffset))
motorSpeed = -_fingers[fingerCounter].TargSpeed; // set to max speed speed depending on direction
else if(abs(_fingers[fingerCounter].CurrErr) <= (proportionalOffset + motorStopOffset))
motorSpeed = (m * (_fingers[fingerCounter].CurrErr + (motorStopOffset * vectorise))) - (_fingers[fingerCounter].MinSpeed * vectorise); // proportional control
// constrain speed to limits
motorSpeed = constrain(motorSpeed,-((signed int)_fingers[fingerCounter].MaxSpeed) ,(signed int)_fingers[fingerCounter].MaxSpeed);
// if motor disabled, set speed to 0
if(!_fingers[fingerCounter].motorEn)
motorSpeed = 0;
// send speed to motors
motorControl(fingerCounter, motorSpeed); // 15us
}
}
void motorStatus(motor_t status)
{
if(0 == status)
{
motorControl(0,0);
}
else
{
if(status > 0)
{
motorControl((abs(status)+5)*10, 0);
}
else
{
motorControl(0, (abs(status)+5)*10);
}
}
}
void tgCPGCableControl::updateTensionSetpoint(double newTension)
{
if (newTension >= 0.0)
{
motorControl().setOffsetTension(newTension);
}
else
{
throw std::runtime_error("Tension setpoint is less than zero!");
}
}
/*
Motor Thread
Handles the roll and pitch motors. It has three states, Follow, Goto Angle and Stop.
Follow = simple updates the motor's angles to the values in the global struct.
Goto = moves the motors to the specified angle over a specified time received from transmitter
Stop = simply stops the motors
*/
void motor_thread(void const *argument) {
struct Values *values = (struct Values*)argument;
char string[5]= {0};
uint32_t mscount=0;
while(1){
//Follow Mode
if(values->follow == 1){
osMutexWait(measureUpdate, osWaitForever);
motorControl(values->pitch, values->roll, global_pitch, global_roll);
osMutexRelease(measureUpdate);
}
//Goto Angle Mode
if(values->follow == 0){
osSignalWait(0x02, osWaitForever);
osMutexWait(measureUpdate, osWaitForever);
measurements.roll += values->rollIncrement;
measurements.pitch += values->pitchIncrement;
motorControl(measurements.pitch,measurements.roll, global_pitch, global_roll);
++mscount;
//Check to see if we reached the desired time
if (mscount >= values->time) {
osTimerStop(timerid_motor); //stop timer
mscount=0; //reset count
// Delay
osDelay(1000);
// Move back to follow mode
measurements.follow = 1;
}
osMutexRelease(measureUpdate);
}
//Stop Mode
if(values->follow == 3){
osDelay(1000);
}
osDelay(MOTOR_DELAY);
}
}
void tgCPGActuatorControl::onStep(tgSpringCableActuator& subject, double dt)
{
m_controlTime += dt;
m_totalTime += dt;
/// @todo this fails if its attached to multiple controllers!
/// is there a way to track _global_ time at this level
// Workaround until we implement PID
tgBasicActuator& m_sca = *(tgCast::cast<tgSpringCableActuator, tgBasicActuator>(subject));
if (m_controlTime >= m_controlStep)
{
m_commandedTension = motorControl().control(m_sca, m_controlTime, controlLength(), getCPGValue());
m_controlTime = 0;
}
else
{
m_sca.moveMotors(dt);
}
}
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line.
* For programmatic remappings you can use a different version of init() which takes
* remappings directly, but for most command-line programs, passing argc and argv is
* the easiest way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "pwm_controller");
ros::NodeHandle n("/pwm_controller");;
n.param("right_p", controlPR, 0.7);
n.param("left_p", controlPL, 0.7);
n.param("right_i", controlIR, 0.001);
n.param("left_i", controlIL, 0.001);
n.param("right_d", controlDR, 0.0);
n.param("left_d", controlDL, 0.0);
ros::Publisher pwm_pub = n.advertise<ras_arduino_msgs::PWM>("/arduino/pwm", 1000);
ros::Subscriber twist_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 1000, updateTwist);
ros::Subscriber encoder_sub = n.subscribe<ras_arduino_msgs::Encoders>("/arduino/encoders", 1000, updateEncoders);
ros::Rate loop_rate(controlRate);
while (ros::ok())
{
pwm_pub.publish(motorControl()); //
ros::spinOnce(); // Run the callbacks.
loop_rate.sleep();
}
return 0;
}
void servoSet(int setPt)
{
// setPt = Angle of Incidence in 10ths of degrees
//char hysteresis = 40;
int servoSpeed;
// Convert angle*10 to a value between 25-93
int tmpSetPt = setPt/4;
if (tmpSetPt > 34) tmpSetPt= 34;
if (tmpSetPt < -34) tmpSetPt=-34;
tmpSetPt += 59;
servoSpeed = (ADCH-tmpSetPt)*20;
if (servoSpeed > 255) servoSpeed= 255;
if (servoSpeed < -255) servoSpeed=-255;
//if (servoSpeed > -60 && servoSpeed < 60) servoSpeed = 0;
//if (servoSpeed > hysteresis) { servoSpeed=255; hysteresis = 10; }
//else if (servoSpeed < -hysteresis) { servoSpeed=-255; hysteresis = 10; }
//else { servoSpeed=0; hysteresis = 100; }
motorControl(servoSpeed);
}
/* is set to 'yes' (#pragma interrupt saveall is generated before the ISR) */
void TI1_OnInterrupt(void)
{
oneMsCounter++;
//-------------------------------------------
g_nspeedControPeriod ++;
speedControlOutput();
//___________________________________________
//-------------------------------------------
g_nDirectionControlPeriod ++;
DirectionControlOutput();
//___________________________________________
if(oneMsCounter == 1)
{
if(AD_Flag) updataSensor(); //读取ADC的值
}
//--------------------------------------------------
else if(oneMsCounter == 2)
{
AngleCalculate(); //计算角度
AngleControl(); //角度的PD值控制
motorControl(); //电机输出控制
}
//-----------------------------------------------
else if(oneMsCounter == 3)
{
g_nSpeedControlCount++;
oneHundredMsCounter++;
if(g_nSpeedControlCount >= SPEED_CONTROL_COUNT)
{
speedControl(); //速度控制
g_nSpeedControlCount=0;
g_nspeedControPeriod=0;
}
if(standFlag){
if(oneHundredMsCounter == 100)
{
if(CAR_SPEED_SET < speedNeed3)
CAR_SPEED_SET += 10;
else
{
CAR_SPEED_SET = speedNeed3;
speedStarEndFlag = 1;
}
oneHundredMsCounter = 0;
}
}
else
{
CAR_SPEED_SET = 0;
oneHundredMsCounter = 0;
speedStarEndFlag = 0;
}
}
//--------------------------------------------------
else if(oneMsCounter == 4)
{
g_nDirectionControlCount ++;
DianciCalculate();
DirectionVoltageSigma();
if(g_nDirectionControlCount >= DIRECTION_CONTROL_COUNT) {
DirectionControl();
g_nDirectionControlCount = 0;
g_nDirectionControlPeriod = 0;
}
}
//-------------------------------------------------
else if(oneMsCounter >= 5)
{
oneMsCounter = 0;
GetMotorPulse();
UartFlag = 1;
}
}
int main(int argc, char *argv[])
{
MotorController motorControl(argc, argv);
}
int16_t main(void) {
init_clock();
init_timer();
init_pin();
init_oc();
init_ui();
InitUSB(); // initialize the USB registers and serial interface engine
while (USB_USWSTAT!=CONFIG_STATE) { // while the peripheral is not configured...
ServiceUSB(); // ...service USB requests
}
// Configure Interrupts on the pic
IEC1bits.CNIE = 1;
CNEN1bits.CN2IE = 1;
IFS1bits.CNIF = 0;
IEC0bits.OC1IE = 1;
IFS0bits.OC1IF = 0;
timer_enableInterrupt(&timer1);
timer_lower(&timer1);
timer_enableInterrupt(&timer2);
timer_lower(&timer2);
timer_enableInterrupt(&timer4);
timer_lower(&timer4);
timer_enableInterrupt(&timer5);
timer_lower(&timer5);
// Configure Pins
inPin0 = &A[0];
pin_analogIn(inPin0);
inPin1 = &A[1];
pin_analogIn(inPin1);
inPin2 = &A[2];
pin_analogIn(inPin2);
inPin3 = &A[3];
pin_analogIn(inPin3);
inPin4 = &A[4];
pin_analogIn(inPin4);
irPin = &A[5];
pin_analogIn(irPin);
outPin = &D[6];
pin_digitalOut(outPin);
oc_pwm(&oc1, outPin, NULL, 10, (uint16_t)(0)); // write to D2 with a 10Hz PWM signal
pin_write(outPin, 10000); //duty doesn't matter, really.
redPin = &D[7];
pin_digitalOut(redPin);
oc_pwm(&oc2, redPin, NULL, 100, (uint16_t)(0));
greenPin = &D[10];
pin_digitalOut(greenPin);
oc_pwm(&oc3, greenPin, NULL, 100, (uint16_t)(0));
bluePin = &D[8];
pin_digitalOut(bluePin);
oc_pwm(&oc4, bluePin, NULL, 100, (uint16_t)(0));
pingPin = &D[4];
pin_digitalOut(pingPin);
oc_pwm(&oc5, pingPin, &timer3, 40000, 0);
receivePin = &D[12];
pin_digitalIn(receivePin);
// Motor controller pins
dirPin = &D[0];
pin_digitalOut(dirPin);
nSleepPin = &D[3];
pin_digitalOut(nSleepPin);
pin_write(nSleepPin, 1);
stepPin = &D[2];
pin_digitalOut(stepPin);
testPin = &D[13];
pin_digitalOut(testPin);
timer_setFreq(&timer1, 100);
while (1) {
ServiceUSB(); // service any pending USB requests
irVoltage = pin_read(irPin);
if (irVoltage < 40000){
dist = 32768;
}
if (irVoltage >= 40000){
dist = 32900;
}
if (dist != stepCount) {
changeFlag += 1;
} else {
changeFlag = 0;
}
if (changeFlag >= 3){
changeFlag = 0;
motorControl(dist);
}
if (touching0 == 10){
greenTarget = 40000;
redTarget = 60000;
blueTarget = 0;
if (currentPetal == 0){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 0;
}
if (touching1 == 11){
greenTarget = 20000;
redTarget = 20000;
blueTarget = 20000;
if (currentPetal == 1){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 1;
}
if (touching2 == 12){
greenTarget = 0;
redTarget = 60000;
blueTarget = 40000;
if (currentPetal == 2){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 2;
}
if (touching3 == 13){
greenTarget = 0;
redTarget = 60000;
blueTarget = 0;
if (currentPetal == 3){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 3;
}
if (touching4 == 14){
greenTarget = 60000;
redTarget = 0;
blueTarget = 0;
if (currentPetal == 4){
greenTarget = 0;
redTarget = 0;
blueTarget = 0;
}
currentPetal == 4;
}
if (greenDuty < greenTarget) {
greenChange = 1;
} else if (greenDuty > greenTarget) {
greenChange = -1;
} else {
greenChange = 0;
onTarget += 1;
}
if (redDuty < redTarget) {
redChange = 1;
} else if (redDuty > redTarget) {
redChange = -1;
} else {
redChange = 0;
onTarget += 1;
}
if (blueDuty < blueTarget) {
blueChange = 1;
} else if (blueDuty > blueTarget) {
blueChange = -1;
} else {
blueChange = 0;
onTarget += 1;
}
greenDuty += greenChange;
redDuty += redChange;
blueDuty += blueChange;
pin_write(greenPin, greenDuty);
pin_write(redPin, redDuty);
pin_write(bluePin, blueDuty);
/*
// fade on when touched
if (touching0 == 10){
if (greenOn == 0){
greenChange = 1;
}
if (greenOn == 1){
greenChange = -1;
}
redChange = -1;
blueChange = -1;
}
if (touching1 == 11){
if (redOn == 1){
redChange = -1;
}
if (redOn == 0){
redChange = 1;
}
blueChange = -1;
greenChange = -1;
}
if (touching2 == 12){
if (blueOn == 1){
blueChange = -1;
}
if (blueOn == 0){
blueChange = 1;
}
greenChange = -1;
redChange = -1;
}
greenDuty = greenDuty + greenChange;
if (greenDuty == MAX_INT){
greenDuty = MAX_INT -1;
greenOn = 1;
greenChange = 0;
}
if (greenDuty == 0){
greenDuty = 1;
greenOn = 0;
greenChange = 0;
}
redDuty = redDuty + redChange;
if (redDuty == MAX_INT){
redDuty = MAX_INT - 1;
redOn = 1;
redChange = 0;
}
if (redDuty == 0){
redDuty = 1;
redOn = 0;
redChange = 0;
}
blueDuty = blueDuty + blueChange;
if (blueDuty == MAX_INT){
blueDuty = MAX_INT - 1;
blueOn = 1;
blueChange = 0;
}
if (blueDuty == 0){
blueDuty = 1;
blueOn = 0;
blueChange = 0;
}
pin_write(greenPin, greenDuty);
pin_write(redPin, redDuty);
pin_write(bluePin, blueDuty);
*/
/*
if (iteration > 10000) {
ping();
iteration = 0;
}
iteration += 1;
*/
}
}
//*MAIN*//
int main(int argc, char * argv[])
{
bool active = true; //flag that controls main loop
int value; //value of received message
char messageType;
unsigned int serialPort;
char * serialAddr = "/dev/ser1";
TCP tcpConnection;
unsigned int clientSocket;
//Initialization
//Parse the arguments
switch (argc)
{
case 2:
serialAddr = argv[1];
break;
}
if ( argc >= 2 )
{
if ( initSerial(serialPort, serialAddr) == false )
{
std::cout << "Serial port initialization failure.\n";
return 0;
}
if ( initMotors() == false )
{
std::cout << "Motor initialization failure.\n";
return 0;
}
}
if ( initTCP(tcpConnection, clientSocket) == false )
{
std::cout << "TCPIP initialization failure.\n";
return 0;
}
while (active)
{
tcpConnection.receiveData( clientSocket,
(char *) &messageType,
sizeof( messageType ));
if(tcpConnection.receiveData( clientSocket,
(char *) &value,
sizeof( value )) == DATA_ERROR)
{
active = false;
std::cout << "Client Disconnected!" << std::endl;
}
#ifdef DEBUG
debugprint(messageType, value);
#endif
if(messageType == 'X')
active = false;
else if (active == true)
{
#ifdef ENABLE_STEERING
if (messageType == 'S')
Move_Motor_Abs(value);
else
#endif
motorControl(serialPort, messageType, value);
//watch out for this!
}
}
std::cout << "Resetting motors..." << std::endl;
resetMotors(serialPort);
#ifdef ENABLE_STEERING
Close_Maxon_Motor_Driver();
#endif
tcpConnection.closeSocket(clientSocket);
std::cout << "Terminating..." << std::endl;
return 0;
}
/*
Main Receive Thread. It reads from the wireless and checks the control part of the packet to determine
what kind of data we are receiving. There are three types PACKET_CTRL1_BEGIN, PACKET_CTRL1_PR, PACKET_CTRL1_RECORD_BEGIN.
PACKET_CTRL1_BEGIN = Receiving a desired Roll and Pitch Angle that the board should travel to over some time
PACKET_CTRL1_PR = simply update the current pitch and roll angle for the board to follow. i.e. real time angle following
PACKET_CTRL1_RECORD_BEGIN = indicate that we are receiving a sequence of angles that the board will follow.
*/
void receive_thread(void const *argument) {
uint8_t status = CC2500_CommandProbe(CC2500_READBIT, CC2500_SRX);
printf("Moved to RX (%x) \n",status);
//osDelay(500);
ring_buffer_t dist_filter;
int size = DIST_FILTER_SIZE;
int dist_buffer[size];
init_buffer(&dist_filter,dist_buffer,size);
float f1, f2, value;
uint16_t control;
uint8_t ctrl = 0;
uint8_t ctrl2 = 0;
while (1) {
control = receive_pitchroll(&f1, &f2, &ctrl2);
//Check to see what kind of Packet we have received
if (control == PACKET_CTRL1_BEGIN) {
while (ctrl != PACKET_CTRL1_END) {
receive_keypad(&ctrl, &value);
//update our global values from the recieved data
switch(ctrl) {
case PACKET_CTRL1_PITCH:
measurements.pitchIncrement = value;
break;
case PACKET_CTRL1_ROLL:
measurements.rollIncrement = value;
break;
case PACKET_CTRL1_TIME:
measurements.time = value;
break;
}
}
//printf("EX MOVE pitchIncrement = %f, rollIncrement = %f, time = %f \n",measurements.pitchIncrement,measurements.rollIncrement,measurements.time);
//Turn off Motor Follow Mode
measurements.follow = 0;
//Start OS Timer for Motor
osTimerStart(timerid_motor,100);
//Wait till follow flag has been reset
while (!measurements.follow) {
osDelay(500);
}
ctrl = 0; // reset control
}
//Update Pitch and Roll angles for motor to follow
else if (control == PACKET_CTRL1_PR) {
measurements.pitch = f1;
measurements.roll = f2;
//Return the signal strength of the wireless receiver.
float f = filter_point(get_signal_strength(), &dist_filter);
showSignalStrength(f); //updates the LEDs
//printf("READ: Pitch = %f (%f) Roll = %f (%f) Control1 = %x Pkt_index = %i Power = %f \n",f1,global_pitch,f2,global_roll,control,ctrl2,f);
}
//Begin playing Recorded Sequence
else if (control == PACKET_CTRL1_RECORD_BEGIN) {
float pitchBuffer[256];
float rollBuffer[256];
float time_interval = f1;
//start receiving the sequence and store in the buffers
receive_record_sequence(pitchBuffer,rollBuffer,&time_interval);
//printf("RECV: Record Sequence: \n");
int i;
measurements.follow = 3; // Stop Motors Following
//printf("Motors moving sequence, timedelay = %f \n",time_interval);
i = 0;
uint8_t j;
float f1, f2;
float pitchInc, rollInc;
//Playing back the Angle Sequence
while (i < 256) {
j = 0;
pitchInc = (pitchBuffer[i+1] - pitchBuffer[i]) / 10;
rollInc = (rollBuffer[i+1] - rollBuffer[i]) / 10;
f1 = pitchBuffer[i];
f2 = rollBuffer[i];
//Linear Interpolation of the Data as we received only 1/10th the actual sequence
//so we fill in the blanks using the average of two consequetive angles in the sequence.
while (j < 10) {
f1 += pitchInc;
f2 += rollInc;
motorControl(f1, f2, global_pitch, global_roll);
printf("Motors moving to angle: index = %i Pitch = %f Roll = %f \n",i,pitchBuffer[i],rollBuffer[i]);
j++;
osDelay(10);//Delay of 100 so the Loop runs at 100Hz, the rate of which the accelerometer reads.
}
//osDelay((int)time_interval);
i++;
}
osDelay(1000);
measurements.follow = 1; // Stop following
}
}
}