File content as of revision 3:4b6080e86761:

/* Karbot motor controller
 * Written by Simon Krogedal
 * 26/05/21
 * Team 9 4th Year project
 * 
 * for NUCLEO-F401RE
 * 
 */

//LIBRARIES
#include "mbed.h"

#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/BatteryState.h>
#include <sensor_msgs/JointState.h>

#include "motor.h"
#include "encoder.h"
#include "MotorControl.h"


//PIN DEFINITIONS
#ifdef TARGET_NUCLEO_F401RE

#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

#else
#error "Check target against pin definitions"
#endif


//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


//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     DEMO            0

/* Functionality Summary

*/

// Create 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);

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))));

// Callback function for a twist message given by the motion controller
void messageCb(const geometry_msgs::Twist& msg);

// Subsribe to cmd_vel
ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);

// 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);


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
    DigitalOut myled(LED1);
    myled = 0;
    
    // Wait until connected to ROS, while blinking LED
    while(!nh.connected()) {
        nh.spinOnce();
        wait_ms(500);
        myled = !myled;
    }
    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();
        
        while (true) {
            leftCtrl.setSpeed(0.5);
            //ThisThread::sleep_for(3s);
            leftCtrl.setSpeed(1.2);
            //ThisThread::sleep_for(3s);
            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);
        
    //}
}