Simon Krogedal
Control project for the Lift-arm. Works with ROS Melodic
Dependencies: mbed Servo ros_lib_melodic ULN2003_StepperDriver Async_4pin_Stepper
Revision 5:71c2f193a7f9, committed 2021-06-03
- Comitter:
- krogedal
- Date:
- Thu Jun 03 21:40:04 2021 +0000
- Parent:
- 4:9edb248c6431
- Commit message:
- Initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/Async_4pin_Stepper.lib Thu Jun 03 21:40:04 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/krogedal/code/Async_4pin_Stepper/#252d645cfc5d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/Servo.lib Thu Jun 03 21:40:04 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/lib/ULN2003_StepperDriver.lib Thu Jun 03 21:40:04 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/fbcosentino/code/ULN2003_StepperDriver/#8a14924886c4
--- a/src/MotorControl.cpp Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,75 +0,0 @@
-/* Karbot motor control class
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
- #include "mbed.h"
- #include "motor.h"
- #include "MotorControl.h"
-
-
-double MotorControl::getError(void) {return (r_clicks - getClicks());}
-
-void MotorControl::algorithm(void) {
-// pc.printf("speed adjusting\n");
- sample_func();
- double error = getError();
- output += Kp * (error + Ti*error - prev_error); //computes current error from encoder sample and the resulting action
- if(output > 1.0) { //limit checks and error flag set
- output = 1.0;
- max_out = 1; //this flag says not to increase speed more
- }
- else if(output < -1.0) {
- output = -1.0;
- max_out = 1;
- }
- else
- max_out = 0;
- mot->setOut(output); //set new output
-// pc.printf("output set to %2.2f\n", output);
- prev_error = error;
-}
-
-MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ti, double ec)
- : encoder(a, b, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ti(ti) {
- output = 0.1; dir = 1; //default to avoid bugs
- max_out = 0; //flag set
- prev_error = 0;
-}
-
-void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed
- r_speed = s;
- r_clicks = s / enc_const;
-// pc.printf("r_clicks set to %2.2f\n", r_clicks);
-}
-
-void MotorControl::drive(void) {
- mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
- mot->drive(); //pc.printf("motor driving\n");
- //pc.printf("setting sampler at period %2.2f\n", period);
- sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
-}
-
-void MotorControl::stop(void) {
- mot->stop();
- sampler.detach();
- prev_error = 0;
-}
-
-void MotorControl::driveManual(void) {
- mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
- mot->drive(); //pc.printf("motor driving\n");
-}
-
-void MotorControl::samplecall(void) {algorithm();}
-
-void MotorControl::setK(double k) {Kp = k;}
-
-void MotorControl::start(void) {} //this function is overridden to avoid bugs
-// double MotorControl::getError(void) {return (speed - getSpeed());}
-double MotorControl::getOutput(void) {return output;}
-bool MotorControl::checkFlag(void) {return max_out;}
\ No newline at end of file
--- a/src/MotorControl.h Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,56 +0,0 @@
-#ifndef KARBOT_MOTOR_CONTROL_H
-#define KARBOT_MOTOR_CONTROL_H
-
-/* Karbot motor control class
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
- #include "mbed.h"
- #include "encoder.h"
- #include "motor.h"
-
-/* This class controls the speed of an individual motor, helping the system keep
- * symmetric with asymmetric components. It is based on the encoder class
- * and contains a pointer to a motor object. With two of these the motion
- * controller can drive the robot to the desired points.
- */
-class MotorControl : public encoder {
- private:
- motor* mot; // motor object
-
- double Kp, // Proportional gain
- Ti, // Integral gain time
- r_speed, // Speed set point
- r_clicks, // Same but in the "encoder language"
- max_speed, // The max speed of the motor (not used)
- output, // PWM output duty cycle
- prev_error; // Previous error, used for intergral control
-
- bool dir, // Drivign direction (not used)
- max_out; // Flag triggered when output is saturated (not used)
-
- double getError(void); // Calculate current error
-
- void algorithm(void); // Control algorithm
-
- public:
- // Constructor takes encoder pins and constants, and a motor object and contorller gains
- MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ti, double ec);
-
- void setSpeed(double s); // Set the speed of the wheel
- void drive(void); // Drive at set speet
- void stop(void); // Stop motor
- void driveManual(void); // Drive at set speed in open loop
- void samplecall(void); // Sample encoder and update read speed value
- void setK(double k); // Set gain (used for Z-N tuning
- void start(void); // This function is overridden to avoid bugs
- double getOutput(void); // Returns current duty cycle
- bool checkFlag(void); // Check whether max out flag is set
-};
-
-#endif
\ No newline at end of file
--- a/src/encoder.cpp Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,198 +0,0 @@
-#ifndef ENCODER_H
-#define ENCODER_H
-
-/* Karbot encoder class
- *
- * This class is based upon the QEI class by Aaron Berk and the encoder class
- * I wrote during ESP in 2nd year
- *
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
-#include "encoder.h"
-
-
-encoder::encoder(
- PinName chanA,
- PinName chanB,
- int CPR,
- bool c,
- double p,
- double ec
- ) : channelA_(chanA, PullUp),
- channelB_(chanB, PullUp),
- c_d(c),
- period(p),
- enc_const(ec) {
-
- pulses_ = 0;
- pulsesPerRev_ = CPR;
- tot_clicks = 0;
- click_rate = 0;
- temp_tot = 0;
-
- //Workout what the current state is.
- int channA = channelA_.read();
- int channB = channelB_.read();
-
- //2-bit state.
- currState_ = (channA << 1) | (channB);
- prevState_ = currState_;
-
- //X4 encoding uses interrupts on channel A,
- //and on channel B.
- channelA_.rise(callback(this, &encoder::encode));
- channelA_.fall(callback(this, &encoder::encode));
- channelB_.rise(callback(this, &encoder::encode));
- channelB_.fall(callback(this, &encoder::encode));
-}
-
-void encoder::reset(void) {
- pulses_ = 0;
-}
-
-int encoder::getCurrentState(void) {
-
- return currState_;
-
-}
-
-int encoder::getPulses(void) {
-
- return pulses_;
-
-}
-
-// +-------------+
-// | X2 Encoding |
-// +-------------+
-//
-// When observing states two patterns will appear:
-//
-// Counter clockwise rotation:
-//
-// 10 -> 01 -> 10 -> 01 -> ...
-//
-// Clockwise rotation:
-//
-// 11 -> 00 -> 11 -> 00 -> ...
-//
-// We consider counter clockwise rotation to be "forward" and
-// counter clockwise to be "backward". Therefore pulse count will increase
-// during counter clockwise rotation and decrease during clockwise rotation.
-//
-// +-------------+
-// | X4 Encoding |
-// +-------------+
-//
-// There are four possible states for a quadrature encoder which correspond to
-// 2-bit gray code.
-//
-// A state change is only valid if of only one bit has changed.
-// A state change is invalid if both bits have changed.
-//
-// Clockwise Rotation ->
-//
-// 00 01 11 10 00
-//
-// <- Counter Clockwise Rotation
-//
-// If we observe any valid state changes going from left to right, we have
-// moved one pulse clockwise [we will consider this "backward" or "negative"].
-//
-// If we observe any valid state changes going from right to left we have
-// moved one pulse counter clockwise [we will consider this "forward" or
-// "positive"].
-//
-// We might enter an invalid state for a number of reasons which are hard to
-// predict - if this is the case, it is generally safe to ignore it, update
-// the state and carry on, with the error correcting itself shortly after.
-void encoder::encode(void) {
-
- int change = 0;
- int chanA = channelA_.read();
- int chanB = channelB_.read();
-
- //2-bit state.
- currState_ = (chanA << 1) | (chanB);
-
- //Entered a new valid state.
- if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
- //2 bit state. Right hand bit of prev XOR left hand bit of current
- //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
- change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
-
- if (change == 0) {
- change = -1;
- }
-
- pulses_ -= change;
- }
-
- prevState_ = currState_;
-
-}
-
-void encoder::sample_func(void) {
- int clicks = getPulses();
- click_rate = ((double)clicks / period);
- reset(); //reset clicks
- tot_clicks += clicks;
- temp_tot += clicks;
-}
-
-double encoder::getClicks(void) {
-// test_sample();
- if(c_d)
- return click_rate;
- else
- return -click_rate;
-}
-
-double encoder::getSpeed(void) {
- double s = click_rate * enc_const; //angular frequency = 2pi*CPS/CPR and v = omega*r
- if(c_d)
- return s;
- else
- return -s;
-}
-
-double encoder::getDistance(void) { //calculates total distance and returns it
-// test_sample();
- double d = ((double)click_store * enc_const);
- if(c_d)
- return d;
- else
- return -d;
-}
-
-double encoder::tempDist(void) { //calculates total distance and returns it
-// test_sample();
- double d = ((double)temp_tot * enc_const);
- if(c_d)
- return d;
- else
- return -d;
-}
-
-void encoder::distRst(void) {
- int temp;
- if(c_d)
- temp = tot_clicks;
- else
- temp = -tot_clicks;
- if(temp > click_store)
- click_store = temp;
- tot_clicks = 0;
-}
-
-void encoder::tempRst(void) {temp_tot = 0;}
-
-void encoder::start(void) {sampler.attach(callback(this, &encoder::sample_func), period);}
-
-#endif
\ No newline at end of file
--- a/src/encoder.h Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,90 +0,0 @@
-#ifndef KARBOT_ENCODER_H
-#define KARBOT_ENCODER_H
-
-/* Karbot encoder class
- *
- * This class is based upon the QEI class by Aaron Berk and the encoder class
- * I wrote during ESP in 2nd year
- *
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
-#include "mbed.h"
-
-
-#define PREV_MASK 0x1 //Mask for the previous state in determining direction
-//of rotation.
-#define CURR_MASK 0x2 //Mask for the current state in determining direction
-//of rotation.
-#define INVALID 0x3 //XORing two states where both bits have changed.
-
-class encoder {
-
- private:
- int tot_clicks, temp_tot; // clicks since last distance reset
- double click_rate, click_store;// clickrate
- bool c_d; // left or right bool
-
- /**
- * Update the pulse count.
- *
- * Called on every rising/falling edge of channels A/B.
- *
- * Reads the state of the channels and determines whether a pulse forward
- * or backward has occured, updating the count appropriately.
- */
- void encode(void);
-
- InterruptIn channelA_;
- InterruptIn channelB_;
-
- int pulsesPerRev_;
- int prevState_;
- int currState_;
-
- volatile int pulses_;
-
- protected:
-
- Ticker sampler; // ticker object to sample speed
- double period, enc_const; // sampling period and wheel constant
- void sample_func(void); // sample function
- double getClicks(void); // returns clickrate
-
- public:
-
- // Constructor takes 3 encoder input pins, CPR, left or right bool, sampling period, and a wheel-size constant
- encoder(PinName chanA, PinName chanB, int CPR, bool c, double p, double ec);
-
- double getSpeed(void); // returns wheel speed
- double getDistance(void); // returns distance travelled
- double tempDist(void); // returns distance travelled
- void distRst(void); // resets distance
- void tempRst(void); // resets distance
- void start(void); // starts recording distance
- void reset(void); // resets counting
-
- /**
- * Read the state of the encoder.
- *
- * @return The current state of the encoder as a 2-bit number, where:
- * bit 1 = The reading from channel B
- * bit 2 = The reading from channel A
- */
- int getCurrentState(void);
-
- /**
- * Read the number of pulses recorded by the encoder.
- *
- * @return Number of pulses which have occured.
- */
- int getPulses(void);
-
-};
-
-#endif
\ No newline at end of file
--- a/src/main.cpp Mon May 31 16:47:02 2021 +0000
+++ b/src/main.cpp Thu Jun 03 21:40:04 2021 +0000
@@ -1,119 +1,157 @@
-/* Karbot motor controller
- * Written by Simon Krogedal
- * 26/05/21
- * Team 9 4th Year project
+/*
+ * Created by Simon Krogedal
+ * 11/05/2021
+ *
*
- * for NUCLEO-F401RE
+ * On launch, run stepper backwards until the contact switch triggered. Set this as joint position
+ * Subscribe to joint topics
+ * Set joint angles when a message is recieved
+ * Publish joint angles
+ * Publish FSR reading
+ *
+ * This version was ported to mbed from the orignal Arduino version
*
*/
-//LIBRARIES
#include "mbed.h"
+// Include Ros libs as needed
#include <ros.h>
-#include <geometry_msgs/Twist.h>
-#include <sensor_msgs/BatteryState.h>
#include <sensor_msgs/JointState.h>
+#include <std_msgs/Float32.h>
-#include "motor.h"
-#include "encoder.h"
-#include "MotorControl.h"
-
-
-//PIN DEFINITIONS
-#ifdef TARGET_NUCLEO_F401RE
+// Lib for hardware interface
+#include "Servo.h"
+#include "Async_4pin_Stepper.h"
-#define L_MOTOR_PIN PC_6
-#define R_MOTOR_PIN PB_14
-#define L_MOTOR_DIR PC_15
-#define R_MOTOR_DIR PC_14
-#define L_ENC_PIN_A PA_14
-#define L_ENC_PIN_B PB_15
-#define R_ENC_PIN_A PA_13
-#define R_ENC_PIN_B PB_1
-#define BAT_A_PIN PA_0
-#define BAT_B_PIN PA_1
+// Pin definitions for NUCLEO-F401RE
+#define stepperPin1 D3 // IN1 for ULN2003 driver
+#define stepperPin2 D4 // IN2 for ULN2003 driver
+#define stepperPin3 D5 // IN3 for ULN2003 driver
+#define stepperPin4 D6 // IN4 for ULN2003 driver
-#else
-#error "Check target against pin definitions"
-#endif
-
+#define servo1Pin D1 // Control pin for Servo 1 on main arm
+#define servo2Pin D2 // Control pin for Servo 2 on crank arm
-//VARIABLES
-#define MOTOR_FREQ 25000.0 // 25 kHz, freq of motor PWM
-#define MOTOR_TS 0.05 // Sample time, ie the time between every PID calculation
-#define MOTOR_KP 0.00002 // Loop motor gain
-#define MOTOR_TI 200 // Integral motor gain
-#define BAT_A_TRIM 1 // Trimming value reporesenting the gain of the voltage monitor
-#define BAT_B_TRIM 1 // Unit is V
+#define SWITCH_PIN D8 // Input pin for digital switch
+#define fsrPin A0 // Input pin for force sensor
-//CONSTANTS
-#define WHEEL_RAD 0.1025 // Radius in meters
-#define WHEEL_BASE 0.400 // Distance between driving wheels in m, affects angular velocity
-#define PI 3.1415 // Mathematical constant
-#define ENCODER_PERIOD 0.05 // Encoder sampling period in seconds (currently 1/20 sec)
-#define ENCODER_CPR 980 // Counts per revolution
-#define L_MAX_SPEED 1.5
-#define R_MAX_SPEED 1.5
+// Define the AccelStepper interface type; 4 wire motor in half step mode
+#define MotorInterfaceType 8
+// Define steps per rev for stepper
+#define SPR 4075.7728
+// Pinion circumference at pitch combined with the above value gives step/m, a useful constant
+#define SPM 128205
+#define SPMM 128.205
+// Degrees per radian, general matemathical constant
+#define DPR 57.3
+// FSR scale value, in N
+#define FSR_SCALE 1
+// Stepper speed, steps/s
+#define STEP_V 833 // (max for 28BYJ-48)
-#define DEMO 0
-
-/* Functionality Summary
-
-*/
-
-// Create node handle
+// Declare node handle
ros::NodeHandle nh;
-// Motor and control objects must be global since they are required in the callback function
-motor leftMot (L_MOTOR_PIN, L_MOTOR_DIR, 1/MOTOR_FREQ);
-motor rightMot (R_MOTOR_PIN, R_MOTOR_DIR, 1/MOTOR_FREQ);
+// Declare hardware objects
+Servo servo1(servo1Pin),
+ servo2(servo2Pin);
+// Initialise stepper with pin sequence IN1-IN3-IN2-IN4
+Async_4pin_Stepper stepper(stepperPin1, stepperPin3, stepperPin2, stepperPin4, SPM);
-MotorControl leftCtrl (L_ENC_PIN_A, L_ENC_PIN_B, NC, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_TI, ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR))));
-MotorControl rightCtrl (R_ENC_PIN_A, R_ENC_PIN_B, NC, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_TI, ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR))));
+// Declare digital input for stepper limit switch
+DigitalIn switchPin(SWITCH_PIN);
-// Callback function for a twist message given by the motion controller
-void messageCb(const geometry_msgs::Twist& msg);
+// Declare publisher and message
+std_msgs::Float32 fsr_val;
+ros::Publisher fsrPublisher("FSR_value", &fsr_val);
+
+// Joint related constants
+//int offset[3] = {0, 0, 0};
+//int restPos[3] = {0, 0, 0};
-// Subsribe to cmd_vel
-ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
+// Like the map function but takes floats as inputs and spits out ints
+int floatmap(float x, float in_min, float in_max, long out_min, long out_max) {
+ float temp = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+ return temp;
+}
-// Messages used for publishing battery voltage and wheel speeds
-sensor_msgs::BatteryState batA_msg;
-sensor_msgs::BatteryState batB_msg;
-sensor_msgs::JointState wheels_msg;
-
-// Publishers
-ros::Publisher batA_pub("battery_pack_A", &batA_msg);
-ros::Publisher batB_pub("battery_pack_B", &batB_msg);
-ros::Publisher wheels_pub("wheel_speeds", &wheels_msg);
+// Function that can be called to calibrate prismatic joint by running it
+// until the switch is activated. Must be run on launch
+void calibrateStepper() {
+ nh.loginfo("Running stepper to limit");
+ // Set target way way back and just overshoot
+ stepper.setTarget(-40000);
+ stepper.goToTarget();
+ // Check switch until triggered
+ while(!switchPin) {
+ wait_ms(10);
+ }
+ stepper.stop();
+ // Set current position as origin
+ stepper.setZeroPos();
+ nh.loginfo("Stepper at 0-pos");
+}
+// Subscriber callback function for message
+void subscriberCallback(const sensor_msgs::JointState& msg) {
+ nh.loginfo("Callback triggered");
+ // Transfrom message for local hardware
+// int s1pos = msg.position[0] * DPR - offset[0];
+// int s2pos = msg.position[1] * DPR - offset[1];
+// int steps = (msg.position[2] - offset[2]) * SPM;
+
+ // Kinematic angles
+// int s1pos = map(msg.position[0], 3.14, 5.85, 36, 145);
+// int s2pos = map(msg.position[1], 2.93, 5.85, 153, 24);
+// int steps = (msg.position[2] - 30) * SPMM;
+
+ // URDF angles - URDF file doesn't use DH, so has a different transform and thus angle limits
+ int s1pos = floatmap(msg.position[0], -1.92, 1.13, 36, 145);
+ int s2pos = floatmap(msg.position[1], -0.44, 2.53, 153, 24);
+ int steps = -(msg.position[2] + 100) * SPMM;
+
+ // Write angles to acutators
+ /*
+ Serial.print("Recieved angles :");
+ for (int i = 0; i < 3; i++) {
+ Serial.print(msg.position[i]);
+ Serial.print(" ");
+ }
+ Serial.print("\nMoving to angles: ");
+ Serial.print(s1pos); Serial.print(" "); Serial.print(s2pos); Serial.print(" "); Serial.println(steps);
+ */
+ servo1.position(s1pos);
+ servo2.position(s2pos);
+ stepper.setTarget(steps);
+ stepper.goToTarget();
+ nh.loginfo("Joints written");
+
+}
+
+// Subscribe to topic /phys_joint_states, which is fed by a script that takes the desired non-parallel joint states
+ros::Subscriber<sensor_msgs::JointState> jointControlSubscriber("phys_joint_states", &subscriberCallback);
+
int main() {
-
- // Initialisation
- nh.initNode();
- nh.subscribe(sub);
-
- // Set up joint state message
- //wheels_msg.name = ["Left wheel","Right wheel"];
- wheels_msg.name[0] = "Left wheel";
- wheels_msg.name[1] = "Right wheel";
-
- // Set up battery messages
- batA_msg.power_supply_technology = 2;
- batA_msg.location = "Slot A";
- batA_msg.design_capacity = 5.2;
- batB_msg.power_supply_technology = 2;
- batB_msg.location = "Slot B";
- batB_msg.design_capacity = 5.2;
-
- // Analogue reading pins for battery voltage
- AnalogIn batA (BAT_A_PIN);
- AnalogIn batB (BAT_B_PIN);
-
- // Built-in lED used as a connection indicator
+
+ // Setup stepper
+ stepper.setSpeed(833);
+
+ // Analogue input for FSR value reading
+ AnalogIn FSR(fsrPin);
+
+ // Center servos on rest positions
+ int centerPos[2] = {42, 142};
+ //Serial.print("\nInitialising servos to "); Serial.print(centerPos[0]); Serial.print(" and "); Serial.println(centerPos[1]);
+ servo1.position(centerPos[0]);
+ servo2.position(centerPos[1]);
+
+ // Set the connection to rosserial
+ nh.initNode();
+
+ // Built-in lED used as a connection indicator
DigitalOut myled(LED1);
myled = 0;
@@ -125,81 +163,27 @@
}
myled = 1;
- // Just drive at various speeds if the demo flag is set
- if(DEMO) {
- nh.loginfo("Running demo script");
- leftCtrl.setSpeed(0.5);
- leftCtrl.driveManual();
+ nh.loginfo("mbed node connected");
+ nh.subscribe(jointControlSubscriber);
+ nh.loginfo("Arduino node subscribed");
+ nh.advertise(fsrPublisher);
+
+ // Find stepper position
+ calibrateStepper();
+
+ // Main loop
+ while(true) {
+
+ // Read FSR pin and publish value
+ fsr_val.data = FSR * FSR_SCALE;
+ fsrPublisher.publish(&fsr_val);
+
+ nh.spinOnce();
- while (true) {
- leftCtrl.setSpeed(0.5);
- //ThisThread::sleep_for(3s);
- leftCtrl.setSpeed(1.2);
- //ThisThread::sleep_for(3s);
+ // Enter a tight for loop for ~1 second
+ for(int i=0; i<1000; i++) {
+ wait_ms(1);
nh.spinOnce();
}
}
-
- // Otherwise initiate main loop
- else {
-
- nh.loginfo("Setup complete");
-
- while(true) {
- // Set built-in LED on if connected to ROS, otherwise off
- if(nh.connected())
- myled = 1;
- else
- myled = 0;
-
- // Measure battery voltages
- batA_msg.voltage = batA * BAT_A_TRIM;
- batB_msg.voltage = batB * BAT_B_TRIM;
-
- // Record wheel speeds
- //wheels_msg.velocity = [leftCtrl.getSpeed(), rightCtrl.getSpeed()];
- wheels_msg.velocity[0] = leftCtrl.getSpeed();
- wheels_msg.velocity[1] = rightCtrl.getSpeed();
-
- // Publish results
- batA_pub.publish(&batA_msg);
- batB_pub.publish(&batB_msg);
- wheels_pub.publish(&wheels_msg);
-
- nh.spinOnce();
-
- // Enter a tight for loop for ~1 second
- for(int i=0; i<1000; i++) {
- wait_ms(1);
- nh.spinOnce();
- }
- }
- }
}
-
-
-void messageCb(const geometry_msgs::Twist& msg) {
-
- nh.logdebug("Twist callback triggered");
-
- /* Calculate v_left and v_right from the v and omega.
- * v comes as linear.x in the twist message, wile omeage is angular.z.
- * v is the average of the wheel speeds, while omega is the difference
- * divided by the wheelbase distance.
- */
-
- /*
- if (msg.angular.z == 0 && msg.linear.x == 0) {
- leftCtrl.stop();
- rightCtrl.stop();
- }
- else {
- */
- double v_l = msg.linear.x - WHEEL_BASE * 0.5 * msg.angular.z;
- double v_r = msg.linear.x + WHEEL_BASE * 0.5 * msg.angular.z;
-
- leftCtrl.setSpeed(v_l);
- rightCtrl.setSpeed(v_r);
-
- //}
-}
--- a/src/motor.cpp Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,53 +0,0 @@
-/* Karbot motor class
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
-
- #include "mbed.h"
- #include "motor.h"
-
-motor::motor(PinName pwm_pin, PinName dir_pin, double period) : output(pwm_pin), dir(dir_pin), T(period) {
- output.period(T); //this period is not going to change during the run
- dir = 1; //forward is active high
- dutCyc = 0.1; //just need a default value to avoid bugs
- stop(); //starts off
-}
-
-void motor::drive(void) {
- driving = 1;
- output.write(dutCyc);
-}
-
-void motor::stop(void) {
- driving = 0;
- output.write(0.0); //just constant low
-}
-
-void motor::setOut(double dc) { //set duty cycle as a number between -1 and 1, where 1 is full force forward, -1 is backwards and 0 is still
- if(dc < 0.0) {
- dir = 0;
- if(dc < -1.0)
- dutCyc = 1.0;
- else
- dutCyc = -dc;
- }
- else {
- dir = 1;
- if(dc > 1.0)
- dutCyc = 1.0;
- else
- dutCyc = dc;
- }
- if(driving)
- output.write(dutCyc);
-}
-
-double motor::getPeriod(void) {return T;}
-
-double motor::getDuty(void) {return dutCyc;}
-
--- a/src/motor.h Mon May 31 16:47:02 2021 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,40 +0,0 @@
-#ifndef KARBOT_MOTOR_H
-#define KARBOT_MOTOR_H
-
-/* Karbot motor class
- * Written by Simon Krogedal
- * 27/05/21
- * Team 9 4th Year project
- *
- * for NUCLEO-F401RE
- *
- */
-
- #include "mbed.h"
-
- class motor {
-
- private:
- PwmOut output; // PWM output pin
- DigitalOut dir; // direction pin
-
- double T, dutCyc; // Period and duty cycle variables
- bool driving; // flag on wheter the motor is driving or not
-
- public:
- // constructor takes 2 pins and the period
- motor(PinName pwm_pin, PinName dir_pin, double period);
-
- void drive(void); // drives at set direction and duty cycle
-
- void stop(void); // stops
-
- void setOut(double dc); // set the output, number between -1 and 1
-
- double getPeriod(void); // returns period
-
- double getDuty(void); // returns dutycycle
-};
-
-
- #endif
\ No newline at end of file