|
| 1 | +/** |
| 2 | + * Arduino SimpleFOC + OSC control example |
| 3 | + * |
| 4 | + * Simple example to show how you can control SimpleFOC via OSC. |
| 5 | + * |
| 6 | + * It's a nice way to work, easier than changing speeds via Seerial text input. It could also be the basis for |
| 7 | + * a functional UI, for example in a lab, art-gallery or similar situation. |
| 8 | + * |
| 9 | + * For this example we used an ESP32 to run the code, a AS5048B I2C absolute encoder |
| 10 | + * and a generic L298 driver board to drive a Emax 4114 gimbal motor. But there is no reason the OSC part will |
| 11 | + * not work with any other setup. |
| 12 | + * |
| 13 | + * You will need: |
| 14 | + * |
| 15 | + * - a working SimpleFOC setup - motor, driver, encoder |
| 16 | + * - a MCU with WiFi and UDP support, for example an ESP32, or an Arduino with Ethernet Shield |
| 17 | + * - a device to run an OSC UI |
| 18 | + * - a configured OSC UI |
| 19 | + * - a WiFi network |
| 20 | + * |
| 21 | + * To do the OSC UI I used TouchOSC from https://hexler.net/products/touchosc |
| 22 | + * There is an example UI file that works with this sketch, see "layout1.touchosc" |
| 23 | + * You can open the UI file in 'TouchOSC Editor' (free program) and transfer it to the TouchOSC app on your device. |
| 24 | + * |
| 25 | + * Alternatively, there are other OSC UIs which may work, e.g. http://opensoundcontrol.org/implementations |
| 26 | + * |
| 27 | + * Using: |
| 28 | + * |
| 29 | + * Change the values below to match the WiFi ssid/password of your network. |
| 30 | + * Load and run the code on your ESP32. Take a note of the IP address of your ESP32. |
| 31 | + * Load and run the UI in TouchOSC. |
| 32 | + * Configure TouchOSC to connect to your ESP32. |
| 33 | + * The first command you send will cause the ESP32 to start sending responses to your TouchOSC device. |
| 34 | + * After this you will see motor position and speed in the UI. |
| 35 | + * Have fun controlling your SimpleFOC motors from your smartphone! |
| 36 | + * |
| 37 | + */ |
| 38 | + |
| 39 | + |
| 40 | +#include "Arduino.h" |
| 41 | +#include <SimpleFOC.h> |
| 42 | + |
| 43 | +#include <WiFi.h> |
| 44 | +#include <WiFiUdp.h> |
| 45 | + |
| 46 | +#include <OSCMessage.h> |
| 47 | +#include <OSCBundle.h> |
| 48 | +#include <OSCBoards.h> |
| 49 | +#include <Math.h> |
| 50 | + |
| 51 | + |
| 52 | +const char ssid[] = "myssid"; // your network SSID (name) |
| 53 | +const char pass[] = "mypassword"; // your network password |
| 54 | +WiFiUDP Udp; |
| 55 | +IPAddress outIp(192,168,1,17); // remote IP (not needed for receive) |
| 56 | +const unsigned int outPort = 8000; // remote port (not needed for receive) |
| 57 | +const unsigned int inPort = 8000; // local port to listen for UDP packets (here's where we send the packets) |
| 58 | + |
| 59 | + |
| 60 | +OSCErrorCode error; |
| 61 | + |
| 62 | +MagneticSensorI2C sensor = MagneticSensorI2C(0x40, 14, 0xFE, 8); |
| 63 | +BLDCMotor motor = BLDCMotor(11); |
| 64 | +BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27); |
| 65 | + |
| 66 | + |
| 67 | +void setup() { |
| 68 | + Serial.begin(115200); |
| 69 | + |
| 70 | + WiFi.begin(ssid, pass); |
| 71 | + |
| 72 | + Serial.print("Connecting WiFi "); |
| 73 | + Serial.println(ssid); |
| 74 | + while (WiFi.status() != WL_CONNECTED) { |
| 75 | + delay(500); |
| 76 | + Serial.print("."); |
| 77 | + } |
| 78 | + Udp.begin(inPort); |
| 79 | + Serial.println(); |
| 80 | + Serial.print("WiFi connected. Local OSC address: "); |
| 81 | + Serial.print(WiFi.localIP()); |
| 82 | + Serial.print(":"); |
| 83 | + Serial.println(inPort); |
| 84 | + |
| 85 | + delay(2000); |
| 86 | + Serial.println("Initializing motor."); |
| 87 | + |
| 88 | + sensor.init(); |
| 89 | + motor.linkSensor(&sensor); |
| 90 | + driver.voltage_power_supply = 9; |
| 91 | + driver.init(); |
| 92 | + motor.linkDriver(&driver); |
| 93 | + motor.controller = ControlType::velocity; |
| 94 | + motor.PID_velocity.P = 0.2; |
| 95 | + motor.PID_velocity.I = 20; |
| 96 | + motor.PID_velocity.D = 0.001; |
| 97 | + motor.PID_velocity.output_ramp = 1000; |
| 98 | + motor.LPF_velocity.Tf = 0.01; |
| 99 | + motor.voltage_limit = 8; |
| 100 | + //motor.P_angle.P = 20; |
| 101 | + motor.init(); |
| 102 | + motor.initFOC(); |
| 103 | + |
| 104 | + Serial.println("All initialization complete."); |
| 105 | +} |
| 106 | + |
| 107 | +// velocity set point variable |
| 108 | +float target_velocity = 2.0; |
| 109 | +// angle set point variable |
| 110 | +float target_angle = 1.0; |
| 111 | + |
| 112 | + |
| 113 | +void motorControl(OSCMessage &msg){ |
| 114 | + if (msg.isInt(0)) |
| 115 | + target_velocity = radians(msg.getInt(0)); |
| 116 | + else if (msg.isFloat(0)) |
| 117 | + target_velocity = radians(msg.getFloat(0)); |
| 118 | + else if (msg.isDouble(0)) |
| 119 | + target_velocity = radians(msg.getDouble(0)); |
| 120 | + Serial.print("Velocity set to "); |
| 121 | + Serial.println(target_velocity); |
| 122 | +} |
| 123 | + |
| 124 | +void cmdControl(OSCMessage &msg){ |
| 125 | + char cmdStr[16]; |
| 126 | + if (msg.isString(0)) { |
| 127 | + msg.getString(0,cmdStr,16); |
| 128 | + String it(cmdStr); |
| 129 | + motor.command(it); |
| 130 | + } |
| 131 | +} |
| 132 | + |
| 133 | +long lastSend = 0; |
| 134 | +OSCMessage bundleIN; |
| 135 | + |
| 136 | +void loop() { |
| 137 | + OSCBundle bundleOUT; |
| 138 | + |
| 139 | + // FOC algorithm function |
| 140 | + motor.move(target_velocity); |
| 141 | + motor.loopFOC(); |
| 142 | + |
| 143 | + |
| 144 | + int size = Udp.parsePacket(); |
| 145 | + if (size > 0) { |
| 146 | + while (size--) { |
| 147 | + bundleIN.fill(Udp.read()); |
| 148 | + } |
| 149 | + if (!bundleIN.hasError()) { |
| 150 | + bundleIN.dispatch("/mot1/S", motorControl); |
| 151 | + bundleIN.dispatch("/mot1/C", cmdControl); |
| 152 | + IPAddress ip = Udp.remoteIP(); |
| 153 | + if (!( ip==outIp )) { |
| 154 | + Serial.print("New connection from "); |
| 155 | + Serial.println(ip); |
| 156 | + outIp = ip; |
| 157 | + } |
| 158 | + } |
| 159 | + else { |
| 160 | + error = bundleIN.getError(); |
| 161 | + Serial.print("error: "); |
| 162 | + Serial.println(error); |
| 163 | + } |
| 164 | + bundleIN.empty(); |
| 165 | + } |
| 166 | + else { // don't receive and send in the same loop... |
| 167 | + long now = millis(); |
| 168 | + if (now - lastSend > 100) { |
| 169 | + int ang = (int)degrees(motor.shaftAngle()) % 360; |
| 170 | + if (ang<0) ang = 360-abs(ang); |
| 171 | + //BOSCBundle's add' returns the OSCMessage so the message's 'add' can be composed together |
| 172 | + bundleOUT.add("/mot1/A").add((int)ang); |
| 173 | + bundleOUT.add("/mot1/V").add((int)degrees(motor.shaftVelocity())); |
| 174 | + Udp.beginPacket(outIp, outPort); |
| 175 | + bundleOUT.send(Udp); |
| 176 | + Udp.endPacket(); |
| 177 | + bundleOUT.empty(); // empty the bundle to free room for a new one |
| 178 | + lastSend = now; |
| 179 | + } |
| 180 | + } |
| 181 | + |
| 182 | +} |
0 commit comments