MorphGI
Basic Mid-Level control for the rebuilt MorphGI control unit, using PWM to communicate with the low level controllers.
Dependencies: ros_lib_kinetic
Diff: main.cpp
- Revision:
- 42:5a5ad23a4bb1
- Parent:
- 36:4459be8296e9
--- a/main.cpp Thu Sep 03 16:30:23 2020 +0000
+++ b/main.cpp Thu Jun 24 20:34:47 2021 +0000
@@ -2,155 +2,841 @@
#include "math.h"
// MBED IMPORTS
#include "mbed.h"
-#include "mbed_events.h"
+
// CUSTOM IMPORTS
#include "MLSettings.h"
-#include "HLComms.h"
-#include "LLComms.h"
+//#include "HLComms.h"
+
+#define N_CHANNELS 4
// DEMAND VARIABLES
-double dblDemandSpeed_mmps[N_CHANNELS] = { 0.0 }; // The linear path velocity (not sent to actuator)
-double dblDemandPosition_mm[N_CHANNELS] = { 0.0 }; // The final target position for the actuator
Serial pc(USBTX, USBRX); // tx, rx for usb debugging
-LLComms llcomms;
-HLComms hlcomms(HL_COMMS_FREQ_HZ);
+
+//HLComms hlcomms(HL_COMMS_FREQ_HZ);
+
+double limDouble(double var, double min, double max){
+ double output = var;
+ if(var <min){
+ output = min;
+ }
+ if (var>max){
+ output = max;
+ }
+ return output;
+}
+
+
+//Ticker setDemandsTicker;
+
+
+//joystick input pins
+
+InterruptIn pinZeroIn (PD_4);
+InterruptIn pinCycleIn (PD_6);
+InterruptIn pinSucIn (PD_7);
+InterruptIn pinGasIn (PF_5);
+InterruptIn pinDebugIn (PF_9);
+InterruptIn pinFlushIn (PG_1);
+
+DigitalIn pinWashIn (PD_5);
+DigitalIn pinFwd (PD_3);
+DigitalIn pinRev (PC_3);
+
+
+AnalogIn pinJSX (PC_0);
+AnalogIn pinJSY (PF_3);
+
+DigitalOut pinFlushOut (PG_0);
+DigitalOut pinGasOut (PD_0);
+DigitalOut pinWashOut (PF_1);
+DigitalOut pinSucOut (PD_1);
+DigitalOut pinThirdPump(PF_2);
+DigitalOut pinJetOut (PF_0);
+
+//DigitalIn arPins[7] = {pinZeroIn, pinCycleIn, pinSucIn, pinWashIn, pinGasIn, pinFwd, pinRev};
+
+Thread threadReadHHC(osPriorityHigh);
+Thread threadSendFeedback(osPriorityNormal);
+
+Semaphore semReadHHC(1);
+Semaphore semSendFeedback(1);
+Semaphore semReceiveAndReplan(1);
+
+//Ticker SendSensorDataTicker;
+Ticker tickerReadHHC;
+Ticker tickerSendFeedback;
+
+void signalReadHHC(){
+ semReadHHC.release();
+}
+
+void signalSendFeedback() {
+ semSendFeedback.release();
+}
+
+//support function states;
+
+bool isZero = 0;
+bool isCycle = 0;
+bool isSuc = 0;
+bool isWash = 0;
+bool isGas = 0;
+bool isFwd = 0;
+bool isRev = 0;
+bool isBusy = 0;
+bool DemPos=0;
+bool isDebug = 0;
+
+//set up PWM output pins for Demand POSITION AND VELOCITY
+PwmOut pinPosOut[N_CHANNELS] = {PB_15, PB_8, PB_9 ,PC_6};
+PwmOut pinVelOut[N_CHANNELS] = {PB_10, PA_0, PB_11 ,PB_3};
+
-Thread threadLowLevelSPI(osPriorityRealtime);
-Thread threadSetDemands(osPriorityNormal);
-Thread threadReceiveAndReplan(osPriorityBelowNormal);
-Thread threadSensorFeedback(osPriorityBelowNormal);
+void FlushActivate(){
+ if(!isBusy){
+ pinFlushOut = 1;
+ }
+}
+
+void FlushStop(){
+ if(!isBusy){
+ pinFlushOut = 0;
+ }
+}
+
+void ZeroActivate(){
+ if(!isBusy){
+ isZero = 1;
+ }
+}
+void ZeroStop(){
+
+}
+
+void DebugActivate(){
+ isDebug = 1;
+}
+
+void DebugStop(){
+ isDebug = 0;
+}
+
+void CycleActivate(){
+ if(!isBusy){
+ isCycle = 1;
+ }
+}
+void CycleStop(){
+
+}
+
+void GasActivate(){
+ if(!isBusy){
+ pinGasOut = 1;
+ }
+}
+void GasStop(){
+ pinGasOut = 0;
+}
+
+void SucActivate(){
+ if(!isBusy){
+ pinSucOut = 1;
+ }
+}
+void SucStop(){
+ pinSucOut = 0;
+}
+
+bool isChange = 1;
+
+Timer timer1;
+
+Ticker DemTicker;
+
+int intAnchorDirection = 0;
+
+
+
+//global variables
+double dblSensitivity = 1.0;
+double dblDriveStep = 0.05;
+bool isSteerLock = 0;
+double dblAlpha;
+double dblBeta;
+double dblTheta;
+double dblPhi;
+double dblS;
+double dblSdot;
+double dblX[3];
+double dblXdot[3];
+double dblAlphaDot;
+double dblBetaDot;
+double dblDeltaAlpha;
+double dblDeltaBeta;
+
+double x_axis_sign = -1.0;
+
+//kinematic constants
+double K_theta = 0.77;// rad/mL
+double d_theta = 0.3;// no unit
-Mutex mutPathIn;
-Semaphore semLLcomms(1);
-Semaphore semSensorData(1);
+void SendFeedback(){
+ //support function interrupt states
+ while(1){
+ semSendFeedback.wait();
+ if(!isBusy){
+ if(pinFwd.read()){
+ isFwd = 1;
+ } else {
+ isFwd = 0;
+ }
+
+ if(pinRev.read()){
+ isRev = 1;
+ } else {
+ isRev = 0;
+ }
+
+ if(pinSucIn.read()){
+ isSuc = 1;
+ } else {
+ isSuc = 0;
+ }
+
+ if(pinWashIn.read()){
+ isWash = 1;
+ } else {
+ isWash = 0;
+ }
+
+ if(pinGasIn.read()){
+ isGas = 1;
+ } else {
+ isGas = 0;
+ }
+
+
+ //
+ //printf("\r\n");
+ //printf("%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t", isZero, isGas, isSuc, isRev, isWash, isCycle, isFwd, isDebug);
+ //
+ // double dblJSXprint = pinJSX.read();
+ // double dblJSYprint = pinJSY.read();
+ // printf("X:%f\tY:%f\t", dblJSXprint, dblJSYprint);
+
+ // printf("%f\t",dblAlphaDot);
+ // printf("%f\t",dblBetaDot);
+ // printf(":: ");
+ //
+ // printf("%f\t",dblAlpha);
+ // printf("%f\t",dblBeta);
+ // printf(":: ");
+ //
+ // printf("%f\t",dblTheta);
+ // printf("%f\t",dblPhi);
+ // printf(":: ");
+ printf("\t");
+
+ printf("%f\t",dblX[1]);
+ printf("%f\t",dblX[0]);
+ printf("%f\t",dblX[2]);
+ printf(":: ");
+
+ // printf("%f\t",dblXdot[0]);
+ // printf("%f\t",dblXdot[2]);
+ // printf("%f\t",dblXdot[1]);
+ // printf(":: ");
+
+ printf("%f\t",dblS);
+ printf("%f\t",dblSdot);
+ printf("\r\n");
+ }
+ }
+}
-Ticker setDemandsTicker;
-Ticker SendSensorDataTicker;
+double DeadZone(double var, double threshold, double max, double min){
+
+ double output = var;
+ output -= 0.5;
+ min -= 0.5;
+ max -= 0.5;
+
+ if(output > threshold){
+ output = (output - threshold);
+ output /= (max - threshold);
+ } else if(output < -1.0*threshold){
+ output = -1.0*(output + threshold);
+ output /= (threshold + min);
+ } else {
+ output = 0.0;
+ }
+
+ output = limDouble(output, -1.0, 1.0);
+ return output;
+}
+
+double ConvertToPWM(double target, double max){
+ double output;
+ output = target*0.8/max ;
+ output += 0.1;
+ return output;
+}
-void sendSensorData() {
- while( true ) {
- semSensorData.wait();
- hlcomms.send_sensor_message(llcomms.positionSensor_uint,llcomms.pressureSensor_uint);
+void SetFrontDemand(double demandPos[], double demandVel[]){
+
+ double demandPos_abs[3];
+ double demandVel_abs[3];
+ double pwmPos_front[3];
+ double pwmVel_front[3];
+
+ for(int ii = 0; ii<3; ii++){
+ demandPos_abs[ii] = abs(demandPos[ii]);
+ demandVel_abs[ii] = abs(demandVel[ii]);
+ pwmPos_front[ii] = ConvertToPWM(demandPos_abs[ii], (double)MAX_ACTUATOR_LIMIT_MM);
+ pwmVel_front[ii] = ConvertToPWM(demandVel_abs[ii], (double)MAX_SPEED_MMPS);
+ pinPosOut[ii].write(pwmPos_front[ii]);
+ pinVelOut[ii].write(pwmVel_front[ii]);
+
+ }
+
+/*
+ //print for debugging
+ printf("X: ");
+ for(int ii = 0; ii<3; ii++){
+ printf("%f\t",pwmPos_front[ii]);
+ }
+ printf("Xdot: ");
+ for(int ii = 0; ii<3; ii++){
+ printf("%f\t",pwmVel_front[ii]);
+ }
+ */
+}
+
+void SetDriveDemand(double drivePos, double driveVel){
+
+ double drivePos_abs;
+ double driveVel_abs;
+ double pwmPos_drive;
+ double pwmVel_drive;
+
+ drivePos_abs = abs(drivePos);
+ driveVel_abs = abs(driveVel);
+
+ pwmPos_drive = ConvertToPWM(drivePos_abs, MAX_DRIVE_SEGMENT_POSITION);
+ pwmVel_drive = ConvertToPWM(driveVel_abs, MAX_DRIVE_SEGMENT_SPEED);
+
+ pinPosOut[3].write(pwmPos_drive);
+ pinVelOut[3].write(pwmVel_drive);
+
+ /*
+ //print for debugging
+ printf("S:%f\tSdot:%f\t",pwmPos_drive,pwmVel_drive);
+ */
+}
+
+double GetMaxTravelTime(double startPositions[], double endPositions[], double speed){
+ double arT[3];
+ for(int ii = 0; ii< 3; ii++){
+ arT[ii] = endPositions[ii] - startPositions[ii];
+ arT[ii] /= speed;
+ arT[ii] = abs(arT[ii]);
+ }
+ double Tmax = 0.0;
+ for(int ii = 0; ii<3; ii++){
+ if( Tmax < arT[ii] ){
+ Tmax = arT[ii];
+ }
+ }
+ return Tmax;
+}
+
+void UpdateFrontSegPosition(double position[]){
+ for(int ii = 0; ii<3; ii++){
+ dblX[ii] = position[ii];
}
}
-void signalSendSensorData() {
- semSensorData.release();
+void GoToNeutralPose(double speed){
+ isBusy = 1;
+ double T_wait;
+ double arSpeed[3] = {speed, speed, speed};
+ double arPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+ SetFrontDemand(arPosition, arSpeed);
+
+ //T_wait = 40.0 / speed;
+ T_wait = GetMaxTravelTime(dblX, arPosition, speed);
+ T_wait += 0.25;//add a little bit of time for security
+ //print for debug
+ printf("\r\nGoing to neutral. %f\r\n", T_wait);
+
+ wait(T_wait);
+ UpdateFrontSegPosition(arPosition);
+ //isZero = 0;
+ isBusy = 0;
+}
+
+void GoToAnchorPose(double speed){
+ //reset front segment position
+ //isAnchor = 0;
+ isBusy = 1;
+ double T_wait;
+ double arSpeed[3] = {speed, speed, speed};
+ double arStartPosition[3];
+ //save starting position
+ for (int ii = 0; ii<3; ii++){
+ arStartPosition[ii] = dblX[ii];
+ }
+
+ double arAnchorPosition[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+
+//Go to neutral
+ GoToNeutralPose(speed);
+
+ //select direction of anchor
+ arAnchorPosition[intAnchorDirection] = ANCHOR_POSITION;
+ intAnchorDirection++; //rotate next anchoring position
+ if(intAnchorDirection >= 3){
+ intAnchorDirection = 0;
+ }
+//go to anchor position
+ SetFrontDemand(arAnchorPosition, arSpeed);
+ T_wait = GetMaxTravelTime(dblX, arAnchorPosition, speed);
+ T_wait += 0.25;//add a little bit of time for security
+ printf("Going to anchor. %f s\r\n", T_wait);
+ wait(T_wait);
+
+ //update front segment position.
+ UpdateFrontSegPosition(arAnchorPosition);
+
+//contract
+ SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
+ T_wait = dblS - HOME_DRIVE_POSITION;
+ T_wait /= SAFE_CONTRACT_SPEED;
+ printf("Contracting. %f s\r\n", T_wait);
+ wait(T_wait);
+ //update drive segment position
+ dblS = HOME_DRIVE_POSITION;
+
+//go back to neutral pose
+ GoToNeutralPose(speed);
+
+//go back to starting position
+ SetFrontDemand(arStartPosition, arSpeed);
+ T_wait = GetMaxTravelTime(dblX, arStartPosition, speed);
+ T_wait += 0.25;//add a little bit of time for security
+ printf("Going to intial position. %f\r\n", T_wait);
+ wait(T_wait);
+ //UpdateFrontSegPosition(arStartPosition);
+ for(int ii = 0; ii<3; ii++){
+ dblX[ii] = arStartPosition[ii];
+ }
+
+ //print for debug
+ isBusy = 0;
+}
+
+
+void GoToCustomPose(double position[], double speed){
+ //reset front segment position
+ //isBusy = 1;
+ double T_wait;
+ double arSpeed[3] = {speed, speed, speed};
+ for (int ii = 0; ii<3; ii++){
+ position[ii] = limDouble(position[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
+ }
+ SetFrontDemand(position, arSpeed);
+
+ //T_wait = 40.0 / speed;
+ T_wait = GetMaxTravelTime(dblX, position, speed);
+ T_wait += 0.25;//add a little bit of time for security
+ //print for debug
+ printf("\r\nGoing to custom pose. T_wait: %f s \r\n", T_wait);
+ wait(T_wait);
+ UpdateFrontSegPosition(position);
+ printf("Done\r\n");
+ //isBusy = 0;
+}
+
+
+void AllGoTo(double position, double speed){
+ //reset front segment position
+ //isBusy = 1;
+ double T_wait;
+ double arSpeed[3] = {speed, speed, speed};
+ position = limDouble(position, 0.0, (double)MAX_ACTUATOR_LIMIT_MM);
+
+ double arPosition[3] = {position, position, position};
+ SetFrontDemand(arPosition, arSpeed);
+
+ //T_wait = 40.0 / speed;
+ T_wait = GetMaxTravelTime(dblX, arPosition, speed);
+ T_wait += 0.25;//add a little bit of time for security
+ //print for debug
+ printf("\r\nMoving all to %f mm. T_wait: %f\r\n",position, T_wait);
+
+ wait(T_wait);
+ UpdateFrontSegPosition(arPosition);
+ printf("Done\r\n");
+ //isBusy = 0;
+}
+
+void FlushSyringes(int repetitions){
+ //isBusy = 1;
+ printf("\r\n\r\n:::Flushing Syringes:::\r\n");
+ pinFlushOut = 1;
+ wait(1.0);
+ for(int ii = 0; ii<repetitions; ii++){
+ AllGoTo(40.0,20.0);
+ wait(0.5);
+ AllGoTo(0.0,2.0);
+ wait(0.5);
+ }
+ pinFlushOut = 0;
+ printf("Done\r\n");
+ //isBusy = 0;
+}
+
+void FlushSegment(int repetitions){
+ //isBusy = 1;
+ printf("\r\n\r\n:::Flushing Segment:::\r\n");
+ FlushSyringes(1);//ensure syringes are flushed.
+ double arZero[3] = {0.0, 0.0, 0.0};
+
+ double flushPosition = 30.0;
+
+ double arFlushPose1[3] = {flushPosition, 0.0, 0.0};
+ double arFlushPose2[3] = {0.0, flushPosition, 0.0};
+ double arFlushPose3[3] = {0.0, 0.0, flushPosition};
+ for(int ii = 0; ii<repetitions; ii++){
+ //flush first chamber
+ printf("Flushing first chamber\r\n");
+ GoToCustomPose(arFlushPose1, SAFE_SPEED_MMpS);
+ AllGoTo(0.0, SAFE_SPEED_MMpS);
+
+ //flush second chamber
+ printf("Flushing second chamber\r\n");
+ GoToCustomPose(arFlushPose2, SAFE_SPEED_MMpS);
+ AllGoTo(0.0, SAFE_SPEED_MMpS);
+
+ //flush third chamber
+ printf("Flushing third chamber\r\n");
+ GoToCustomPose(arFlushPose3, SAFE_SPEED_MMpS);
+ AllGoTo(0.0, SAFE_SPEED_MMpS);
+
+ FlushSyringes(1);//flush syringes
+ }
+ printf("Done\r\n");
+ //isBusy = 0;
}
-// This function will be called when a new transmission is received from high level
-void ReceiveAndReplan() {
+void FlushTubing(int repetitions){
+ //isBusy = 1;
+ printf("\r\n\r\n:::Flushing Tubing:::\r\n");
+ for (int ii = 0; ii<repetitions; ii++){
+ AllGoTo(40.0,15.0);
+ pinFlushOut = 1;
+ wait(1.0);
+ AllGoTo(0.0, 2.0);
+ pinFlushOut = 0;
+ wait(0.5);
+ }
+}
- SendSensorDataTicker.attach(&signalSendSensorData, 1/(float)SENSOR_FEEDBACK_HZ); // Set up planning thread to recur at fixed intervals
+
+void DebugMode(){
+ isBusy = 1;
+ printf("\r\nEntering debug mode. Going to zero.\r\n");
+ double zeroPos[3] = {0.0, 0.0, 0.0};
+ double safeVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
- struct demands_struct input;
- DigitalOut SupportPins[4] = {PE_4, PE_2, PE_3, PE_6};
+
+ SetFrontDemand(zeroPos, safeVel);
+ SetDriveDemand(0.0, SAFE_CONTRACT_SPEED);
+ double T_wait = GetMaxTravelTime(dblX, zeroPos, SAFE_SPEED_MMpS);
+ T_wait += 0.25;
+ wait(T_wait);
+ UpdateFrontSegPosition(zeroPos);
+ printf("Done\r\nPlease enter command\r\n");
+
+ int countSyringeFlush = 0;
+ int countSegmentFlush = 0;
+ int countTubingFlush = 0;
- while( true ) {
- hlcomms.newData.wait();
- input = hlcomms.get_demands();
- // SUPPORT FUNCTIONS
- // [isInsufflate,isSuction,isWashLens,isJet]
- for(short int i=0; i<4; i++) {
- if(i<2) { // Active low, i.e. 0 = Off
- SupportPins[i].write((short int)input.utitilies_bool[i]);
- } else { // Active high, i.e. 1 = Off
- SupportPins[i].write((short int)(!input.utitilies_bool[i]));
+ while(isDebug){
+ isBusy = 1;
+ //printf("x\r\n");
+
+ if( pinZeroIn.read() ){
+ //printf("zero%d\r\n",countSyringeFlush);
+
+ if(countSyringeFlush == 0){
+ printf("\r\n Syringe Flush Selected\r\n");
+ }
+ countSyringeFlush++;
+ countSegmentFlush = 0;
+ countTubingFlush = 0;
+ if(countSyringeFlush > 50){
+ FlushSyringes(4);
+ countSyringeFlush = 0;
}
+ //printf("%d\r\n", countSyringeFlush);
+
+ }else {
+ countSyringeFlush = 0;
+ }
+
+ if( pinSucIn.read() ){
+ //printf("suc%d\r\n", countSegmentFlush);
+
+ if(countSegmentFlush == 0){
+ printf("\r\n Segment Flush Selected\r\n");
+ }
+ countSyringeFlush = 0;
+ countSegmentFlush++;
+ countTubingFlush = 0;
+ if(countSegmentFlush > 50){
+ FlushSegment(2);
+ countSegmentFlush = 0;
+ }
+ }else {
+ countSegmentFlush = 0;
+ }
+
+ if( pinWashIn.read() ){
+ //printf("wash%d\r\n", countTubingFlush);
+ if(countTubingFlush == 0){
+ printf("\r\n Tubing Flush Selected\r\n");
+ }
+ countSyringeFlush = 0;
+ countSegmentFlush = 0;
+ countTubingFlush++;
+ if(countTubingFlush > 50){
+ FlushTubing(2);
+ countTubingFlush = 0;
+ }
+ }else {
+ countTubingFlush = 0;
}
- // PROCESS INPUT
- double maxTimesToTarget_s[3] = { -1.0 };
- //[8,7,6,4,3,-1,-1,0,-1]
- //FRONT = channels 0,1,2
- //MID = channels 3,4(,5)
- //REAR = channels (6,)7(,8)
- // Lock mutex, preventing setDemandsForLL from running
- mutPathIn.lock();
- for(short int segNum=0; segNum<3; segNum++) {
- // Limit requested speed
- if( input.speeds_mmps[segNum] > 0 ) {
- double limitedSpeed_mmps = min( max( 0.0 , input.speeds_mmps[segNum] ) , (double)MAX_SPEED_MMPS );
- // For each actuator, limit the input position, calculate the position change, and select the absolute max
- double dblDistanceToTarget_mm[3] = { -1.0 };
- double maxDistanceToTarget_mm = 0.0;
- for(short int i=0; i<3; i++) {
- short int channel = segNum*3+i;
- if(channel>=N_CHANNELS) {
- continue;
+
+ wait(0.1);
+ }
+
+ GoToNeutralPose(SAFE_SPEED_MMpS);
+ //reset front segment position
+ dblAlpha = 0.0;
+ dblBeta = 0.0;
+ printf("\r\nManual control enabled\r\n");
+ isBusy = 0;
+}
+
+void ReadHHC(){
+ printf("Homing. Please wait...\r\n");
+
+
+ double startingPos[3] = {ACTUATOR_OFFSET, ACTUATOR_OFFSET, ACTUATOR_OFFSET};
+ double startingVel[3] = {SAFE_SPEED_MMpS, SAFE_SPEED_MMpS, SAFE_SPEED_MMpS};
+ SetFrontDemand(startingPos, startingVel);
+ SetDriveDemand(HOME_DRIVE_POSITION, SAFE_CONTRACT_SPEED);
+ wait(5);
+ printf("Done.\r\n");
+ threadSendFeedback.start(SendFeedback);
+ tickerSendFeedback.attach(&signalSendFeedback, 1/(float)SENSOR_FEEDBACK_HZ);
+ while(1){
+ semReadHHC.wait();
+
+ if(isZero){
+ GoToNeutralPose(SAFE_SPEED_MMpS);
+ //reset front segment position
+ dblAlpha = 0.0;
+ dblBeta = 0.0;
+ isZero = 0;
+ printf("Done\r\n");
+ }
+
+ if(isCycle){
+ GoToAnchorPose(SAFE_SPEED_MMpS);
+ isCycle = 0;
+ printf("Done\r\n");
+ }
+ if(isDebug){
+ DebugMode();
+ }
+
+ if(!isSteerLock){
+ //read Joystick sensors
+ double dblJSX = pinJSX.read();
+ double dblJSY = pinJSY.read();
+
+ dblJSX = DeadZone(dblJSX, 0.07, 0.85, 0.15);
+ dblJSY = DeadZone(dblJSY, 0.07, 0.85, 0.15);
+
+ if(pinWashIn.read()){
+ pinWashOut = 0;
+ }else{
+ pinWashOut = 1;
+ }
+
+ //convert to angular velocity for front segment
+
+ //Find changes in angle
+ dblDeltaAlpha = dblJSX * x_axis_sign * dblSensitivity * MAX_STEER_SPEED_RADpS / HHC_READ_FREQ_HZ;
+ dblDeltaBeta = dblJSY*dblSensitivity*MAX_STEER_SPEED_RADpS/HHC_READ_FREQ_HZ;
+ //convert to speeds in [rad/s]
+ dblAlphaDot = dblDeltaAlpha*HHC_READ_FREQ_HZ;
+ dblBetaDot = dblDeltaBeta*HHC_READ_FREQ_HZ;
+
+ //calculate required velocity based on current possition and new inputs.
+ dblPhi = atan2(dblBeta,dblAlpha);
+ double dblPhiDot = dblAlpha * dblBetaDot - dblBeta * dblAlphaDot;
+ dblPhiDot /= (dblAlpha * dblAlpha + dblBeta*dblBeta);
+ if( fabs( cos(dblPhi) ) >= 0.7106781 ){
+ dblTheta = dblAlpha / cos(dblPhi);
+ double dblThetaDot = dblAlphaDot * cos(dblPhi) + dblAlpha * dblPhiDot * sin(dblPhi);
+ dblThetaDot /= cos(dblPhi)*cos(dblPhi);
+ double dblPsi[3];
+ for (int ii =0; ii<3; ii++){
+ dblPsi[ii] = dblPhi - ii*2*PI/3;
+ if(dblBeta == 0){
+ dblXdot[ii] = ACT_CONV * K_theta*( cos(dblPsi[ii]) + d_theta);
+ dblXdot[ii] *= ( dblAlphaDot - dblBetaDot * sin(dblPsi[ii]) ) / cos(dblPhi);
}
- double dblCurrentPosition_mm = llcomms.positionSensor_mm[channel];
- if(input.psi_mm[channel]<0 || dblCurrentPosition_mm<0) { // If requested position is negative or the sensor feedback is erroneous
- continue;
+ else{
+ dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
+ }
+ }
+ }
+ else {
+ dblTheta = dblBeta / sin(dblPhi);
+ double dblThetaDot = dblBetaDot * sin(dblPhi) + dblBeta * dblPhiDot * cos(dblPhi);
+ dblThetaDot /= sin(dblPhi)*sin(dblPhi);
+ double dblPsi[3];
+ for (int ii =0; ii<3; ii++){
+ dblPsi[ii] = dblPhi - ii*2*PI/3;
+ if(dblAlpha == 0){
+ dblXdot[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta);
+ dblXdot[ii] *= ( dblBetaDot - dblAlphaDot * sin(dblPsi[ii]) ) / sin(dblPhi);
}
- // Limit actuator position
- input.psi_mm[channel] = min( max( 0.0 , input.psi_mm[channel] ) , (double)MAX_ACTUATOR_LIMIT_MM );
- // Calculate actuator position change
- dblDistanceToTarget_mm[i] = fabs(input.psi_mm[channel] - dblCurrentPosition_mm);
- // Select the max absolute actuator position change
- if(dblDistanceToTarget_mm[i]>maxDistanceToTarget_mm) {
- maxDistanceToTarget_mm = dblDistanceToTarget_mm[i];
+ else{
+ dblXdot[ii] = ACT_CONV * K_theta * ( ( cos(dblPsi[ii]) + d_theta ) * dblThetaDot - dblTheta * sin(dblPsi[ii]) * dblPhiDot);
}
}
- // For max actuator position change, calculate the time to destination at the limited speed
- maxTimesToTarget_s[segNum] = fabs(maxDistanceToTarget_mm) / limitedSpeed_mmps;
- // For each actuator, replan target position and velocity as required
- for(short int i=0; i<3; i++) {
- short int channel = segNum*3+i;
- // If requested actuator position change is already within tolerance, do NOT replan that actuator
- if( dblDistanceToTarget_mm[i] <= 0.0 ) continue;
- // Calculate velocity for each motor to synchronise movements to complete in max time
- // Set dblDemandPosition_mm and dblDemandSpeed_mmps
- dblDemandPosition_mm[channel] = input.psi_mm[channel];
- dblDemandSpeed_mmps[channel] = dblDistanceToTarget_mm[i] / maxTimesToTarget_s[segNum];
+ }
+ //calculate new angles for next front segment position
+ dblAlpha += dblDeltaAlpha; // update new alpha postion
+ dblBeta += dblDeltaBeta; // update new beta position
+
+ dblAlpha = limDouble(dblAlpha, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
+ dblBeta = limDouble(dblBeta, (double)-1.0*MAX_THETA_RAD, (double)MAX_THETA_RAD);
+ dblPhi = atan2(dblBeta,dblAlpha); // update new phi position
+ if( fabs( cos(dblPhi) ) >= 0.7106781 ){
+ dblTheta = dblAlpha / cos(dblPhi);//calculate new theta value
+ }
+ else {
+ dblTheta = dblBeta / sin(dblPhi);
+ }
+ dblTheta = limDouble(dblTheta, 0.0, (double)MAX_THETA_RAD ); //limit angle
+ //update alpha and beta if limit has occured
+ dblAlpha = dblTheta *cos(dblPhi);
+ dblBeta = dblTheta *sin(dblPhi);
+
+ double dblPsi[3];
+ for (int ii = 0; ii <3; ii++) {//calculate new acuator positions
+ dblPsi[ii] = dblPhi - ii*2*PI/3;
+ dblX[ii] = ACT_CONV * K_theta * ( cos(dblPsi[ii]) + d_theta) * dblTheta;
+ //add offset
+ dblX[ii] += ACTUATOR_OFFSET;
+
+ //limit positions
+ dblX[ii] = limDouble( dblX[ii], 0.0, (double)MAX_ACTUATOR_LIMIT_MM );
+ }
+ //calculate new drive segment position
+ //read drive segment buttons
+ //double dblDriveStep = 0.4;
+
+ //printf("pin:%d\t", pinFwd.read());
+
+ if(pinFwd.read()){
+ if(!pinRev.read()){
+ dblSdot = SAFE_CONTRACT_SPEED;
+ } else{
+ dblSdot = 0.0;
}
} else {
- maxTimesToTarget_s[segNum] = -1.0;
+ if(pinRev.read()){
+ dblSdot = -1.0*SAFE_CONTRACT_SPEED;
+ } else{
+ dblSdot = 0.0;
+ }
}
+
+ dblS += dblSdot/HHC_READ_FREQ_HZ;
+ //limit extension (uses word position, but actually refers to pressure)
+ dblS = limDouble( dblS, 0.0, (double)MAX_EXTENSION_PRESSURE );
+
+// printf("S:%f\tSdot:%f\t", dblS, dblSdot);
+ //Send signals to actuators.
+ SetFrontDemand(dblX, dblXdot);//front segment
+ SetDriveDemand(dblS, dblSdot);//drive segment
+
+
}
- // Unlock mutex, allowing setDemandsForLL to run again
- mutPathIn.unlock();
-
- // SEND MESSAGE
- hlcomms.send_durations_message(maxTimesToTarget_s);
}
-
-}
-
-void startLLcomms() { // Send new demands to LL after receiving new target data
- semLLcomms.release(); // Uses threadSetDemands which is normal priority
}
-void setDemandsForLL() {
-
- while(1) {
- semLLcomms.wait();
- mutPathIn.lock(); // Lock relevant mutex
- for(short int i=0; i<N_CHANNELS; i++) { // For each LL
- llcomms.mutChannel[i].lock(); // MUTEX LOCK
- llcomms.demandPosition_mm[i] = dblDemandPosition_mm[i];
- llcomms.demandSpeed_mmps[i] = dblDemandSpeed_mmps[i];
- llcomms.mutChannel[i].unlock(); // MUTEX UNLOCK
- llcomms.isDataReady[i] = 1; // Signal that data ready
- } // end for
- mutPathIn.unlock(); // Unlock relevant mutex
- } // end while(1)
-
+void SetUp(){
+ for(int ii = 0; ii<N_CHANNELS; ii++){
+ pinPosOut[ii].write(0.0);
+ pinVelOut[ii].write(0.0);
+ }
}
int main() {
+ pinThirdPump = 1;
+ pinJetOut = 1;
+ pinWashOut = 1;
pc.baud(BAUD_RATE);
- //printf("ML engage. Compiled at %s\r\n.",__TIME__);
- wait(3);
+ for(int ii = 0; ii<N_CHANNELS; ii++){
+ pinPosOut[ii].period_ms(2.0);
+ pinVelOut[ii].period_ms(2.0);
+ }
+ wait(1.0);
+ printf("ML engage. Compiled at %s\r\n",__TIME__);
+
+
+ //set up support function interrupts
+ //Rising
- threadLowLevelSPI.start(callback(&llcomms.queue, &EventQueue::dispatch_forever)); // Start the event queue
- threadReceiveAndReplan.start(ReceiveAndReplan);// Start replanning thread
- threadSetDemands.start(setDemandsForLL); // Start planning thread
- threadSensorFeedback.start(sendSensorData); // Start sensor feedback thread
+ pinZeroIn.mode(PullNone);
+ pinCycleIn.mode(PullNone);
+ pinWashIn.mode(PullNone);
+ pinGasIn.mode(PullNone);
+ pinSucIn.mode(PullNone);
+ pinFwd.mode(PullNone);
+ pinRev.mode(PullNone);
+ pinFlushIn.mode(PullDown);
+ pinDebugIn.mode(PullDown);
+
+ pinZeroIn.rise(&ZeroActivate);
+ pinCycleIn.rise(&CycleActivate);
+ pinFlushIn.rise(&FlushActivate);
+ pinSucIn.rise(&SucActivate);
+ pinGasIn.rise(&GasActivate);
+ pinDebugIn.rise(&DebugActivate);
+ //falling
+ pinZeroIn.fall(&ZeroStop);
+ pinCycleIn.fall(&CycleStop);
+ pinFlushIn.fall(&FlushStop);
+ pinSucIn.fall(&SucStop);
+ pinGasIn.fall(&GasStop);
+ pinDebugIn.fall(&DebugStop);
+ threadReadHHC.start(ReadHHC);
- setDemandsTicker.attach(&startLLcomms, 1/(float)LL_DEMANDS_FREQ_HZ); // Set up LL comms thread to recur at fixed intervals
- Thread::wait(1);
+ tickerReadHHC.attach(&signalReadHHC, 1/(float)HHC_READ_FREQ_HZ); // set up
+
+
while(1) {
Thread::wait(osWaitForever);
}