main.cpp

00001 /*
00002  * Created by Simon Krogedal
00003  * 11/05/2021
00004  * 
00005  * 
00006  * On launch, run stepper backwards until the contact switch triggered. Set this as joint position
00007  * Subscribe to joint topics
00008  * Set joint angles when a message is recieved
00009  * Publish joint angles
00010  * Publish FSR reading
00011  * 
00012  * This version was ported to mbed from the orignal Arduino version
00013  * 
00014  */
00015 
00016 #include "mbed.h"
00017 
00018 // Include Ros libs as needed
00019 #include <ros.h>
00020 #include <sensor_msgs/JointState.h>
00021 #include <std_msgs/Float32.h>
00022 
00023 // Lib for hardware interface
00024 #include "Servo.h"
00025 #include "Async_4pin_Stepper.h"
00026 
00027 // Pin definitions for NUCLEO-F401RE
00028 #define stepperPin1   D3     // IN1 for ULN2003 driver
00029 #define stepperPin2   D4     // IN2 for ULN2003 driver
00030 #define stepperPin3   D5     // IN3 for ULN2003 driver
00031 #define stepperPin4   D6     // IN4 for ULN2003 driver
00032 
00033 #define servo1Pin     D1     // Control pin for Servo 1 on main arm
00034 #define servo2Pin     D2     // Control pin for Servo 2 on crank arm
00035 
00036 #define SWITCH_PIN    D8     // Input pin for digital switch
00037 #define fsrPin        A0     // Input pin for force sensor
00038 
00039 
00040 // Define the AccelStepper interface type; 4 wire motor in half step mode
00041 #define MotorInterfaceType 8
00042 // Define steps per rev for stepper
00043 #define SPR 4075.7728
00044 // Pinion circumference at pitch combined with the above value gives step/m, a useful constant
00045 #define SPM 128205
00046 #define SPMM 128.205
00047 // Degrees per radian, general matemathical constant
00048 #define DPR 57.3
00049 // FSR scale value, in N
00050 #define FSR_SCALE 1
00051 // Stepper speed, steps/s
00052 #define STEP_V 833 // (max for 28BYJ-48)
00053 
00054 // Declare node handle
00055 ros::NodeHandle nh;
00056 
00057 // Declare hardware objects
00058 Servo   servo1(servo1Pin),
00059         servo2(servo2Pin);
00060 // Initialise stepper with pin sequence IN1-IN3-IN2-IN4
00061 Async_4pin_Stepper stepper(stepperPin1, stepperPin3, stepperPin2, stepperPin4, SPM);
00062 
00063 // Declare digital input for stepper limit switch
00064 DigitalIn switchPin(SWITCH_PIN);
00065 
00066 // Declare publisher and message
00067 std_msgs::Float32 fsr_val;
00068 ros::Publisher fsrPublisher("FSR_value", &fsr_val);
00069 
00070 // Joint related constants
00071 //int offset[3] = {0, 0, 0};
00072 //int restPos[3] = {0, 0, 0};
00073 
00074 // Like the map function but takes floats as inputs and spits out ints
00075 int floatmap(float x, float in_min, float in_max, long out_min, long out_max) {
00076   float temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00077   return temp;
00078 }
00079 
00080 // Function that can be called to calibrate prismatic joint by running it 
00081 // until the switch is activated. Must be run on launch
00082 void calibrateStepper() {
00083   nh.loginfo("Running stepper to limit");
00084   // Set target way way back and just overshoot
00085   stepper.setTarget(-40000);
00086   stepper.goToTarget();
00087   // Check switch until triggered
00088   while(!switchPin) {
00089     wait_ms(10);
00090   }
00091   stepper.stop();
00092   // Set current position as origin
00093   stepper.setZeroPos();
00094   nh.loginfo("Stepper at 0-pos");
00095 }
00096 
00097 
00098 // Subscriber callback function for message
00099 void subscriberCallback(const sensor_msgs::JointState& msg) {
00100   nh.loginfo("Callback triggered");
00101   // Transfrom message for local hardware
00102 //  int s1pos = msg.position[0] * DPR - offset[0];
00103 //  int s2pos = msg.position[1] * DPR - offset[1];
00104 //  int steps = (msg.position[2] - offset[2]) * SPM;
00105 
00106   // Kinematic angles
00107 //  int s1pos = map(msg.position[0], 3.14, 5.85, 36, 145);
00108 //  int s2pos = map(msg.position[1], 2.93, 5.85, 153, 24);
00109 //  int steps = (msg.position[2] - 30) * SPMM;
00110   
00111   // URDF angles - URDF file doesn't use DH, so has a different transform and thus angle limits
00112   int s1pos = floatmap(msg.position[0], -1.92, 1.13, 36, 145);
00113   int s2pos = floatmap(msg.position[1], -0.44, 2.53, 153, 24);
00114   int steps = -(msg.position[2] + 100) * SPMM;
00115 
00116   // Write angles to acutators
00117   /*
00118   Serial.print("Recieved angles :");
00119   for (int i = 0; i < 3; i++) {
00120     Serial.print(msg.position[i]);
00121     Serial.print(" ");
00122   }
00123   Serial.print("\nMoving to angles: ");
00124   Serial.print(s1pos); Serial.print(" "); Serial.print(s2pos); Serial.print(" "); Serial.println(steps);
00125   */
00126   servo1.position(s1pos);
00127   servo2.position(s2pos);
00128   stepper.setTarget(steps);
00129   stepper.goToTarget();
00130   nh.loginfo("Joints written");
00131 
00132 }
00133 
00134 // Subscribe to topic /phys_joint_states, which is fed by a script that takes the desired non-parallel joint states
00135 ros::Subscriber<sensor_msgs::JointState> jointControlSubscriber("phys_joint_states", &subscriberCallback);
00136 
00137 int main() {
00138   
00139   // Setup stepper
00140   stepper.setSpeed(833);
00141   
00142     // Analogue input for FSR value reading
00143     AnalogIn FSR(fsrPin);
00144 
00145   // Center servos on rest positions
00146   int centerPos[2] = {42, 142};
00147   //Serial.print("\nInitialising servos to "); Serial.print(centerPos[0]); Serial.print(" and "); Serial.println(centerPos[1]);
00148   servo1.position(centerPos[0]);
00149   servo2.position(centerPos[1]);
00150 
00151   // Set the connection to rosserial
00152   nh.initNode();
00153 
00154   // Built-in lED used as a connection indicator
00155     DigitalOut myled(LED1);
00156     myled = 0;
00157     
00158     // Wait until connected to ROS, while blinking LED
00159     while(!nh.connected()) {
00160         nh.spinOnce();
00161         wait_ms(500);
00162         myled = !myled;
00163     }
00164     myled = 1;
00165     
00166   nh.loginfo("mbed node connected");
00167   nh.subscribe(jointControlSubscriber);
00168   nh.loginfo("Arduino node subscribed");
00169   nh.advertise(fsrPublisher);
00170 
00171   // Find stepper position
00172   calibrateStepper();
00173   
00174   // Main loop
00175   while(true) {
00176       
00177         // Read FSR pin and publish value
00178         fsr_val.data = FSR * FSR_SCALE;
00179         fsrPublisher.publish(&fsr_val);
00180             
00181         nh.spinOnce();
00182         
00183         // Enter a tight for loop for ~1 second
00184         for(int i=0; i<1000; i++) {
00185             wait_ms(1);
00186             nh.spinOnce();
00187         }
00188     }
00189 }