David Pasztor
/
Motor_control
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 45:bfd7cbd41957
- Parent:
- 44:0b92f72641d7
- Child:
- 46:5c50778bb2d5
--- a/main.cpp Sat Mar 18 21:35:45 2017 +0000
+++ b/main.cpp Sun Mar 19 23:46:22 2017 +0000
@@ -7,10 +7,16 @@
#include "motorControl.h"
#include "parser.h"
+//#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true
+
Mutex mutex;
+Thread motorControlThread;
+
+Ticker monitorTicker;
+void monitor();
volatile float w3 = 0; //Angular velocity
-volatile float duty = 0.5;
+volatile float duty = 0.2;
volatile int count_i3 = 0;
volatile char userInput[256];
@@ -20,23 +26,37 @@
void rotateWith(float r, float v) {}
-void setVelocity(float v) {}
void playTunes(const vector<float>& tunes) {}
-void serialThread() {
- while(true) {
- if (readyForCommand) {
- scanf("%s", userInput);
- ParseResult curr = parse((char *) userInput);
- if (curr.success) {
- mutex.lock();
- commandReady = true;
- parseResult = curr;
- mutex.unlock();
- }
- }
- }
-}
+volatile char userOutput[256];
+volatile bool outputRequested=false;
+
+//void serialOut() {
+// while (true) {
+// if (outputRequested) {
+//// mutex.lock();
+// printf("%s\n\r", userOutput);
+// outputRequested = false;
+//// mutex.unlock();
+// }
+// }
+//}
+//void runMotor() {
+// while (true) {
+// motorOut((state-orState+lead+6)%6, duty);
+// Thread::wait(5);
+// }
+// print("On");
+//}
+
+//void motorSwitchOn() {
+// motorControlThread.start(callback(runMotor));
+//}
+//
+//void motorSwitchOff() {
+// motorControlThread.terminate();
+//}
+
volatile int CHA_state = 0x00;
volatile int CHB_state = 0x00;
@@ -46,9 +66,10 @@
volatile float diskPosition = 0.0; //in degrees
Timer dt_I3;
-Timer motorTimer;
Ticker controlTicker;
Ticker motorOutTicker;
+Ticker lifeTicker;
+
volatile float currentRevs = 0.0; //number of revs done
volatile float goalRevs = 10.0; //number of revs to do
volatile float prevError = 0.0; //previous error in position
@@ -59,75 +80,114 @@
#define kd 0.02f
#define k 10.0f
-const float VKp = 0.5f;
-const float VKi = 0.0f;
-const float VKd = 0.0f;
-volatile float vPrevError = 0.0f;
-volatile float targetV = 300.0f;
+
+
//given in ms,used to call the PD controller
-const float dtControl = 0.002f;
-
+const float dtControl = 0.2f;
// Period of giving power to the motor
-const float dtMotor = 0.005f;
+const float dtMotor = 0.05f;
volatile bool commandFinished = false;
-void runMotor() {
- motorOut((state-orState+lead+6)%6, duty);
+float min(float a, float b) {
+ return a<b ? a : b;
+}
+
+float computeDelta(float p) {
+ float dMin = 0.1;
+ float dMax = 0.4;
+
+ if (p > dMax)
+ return dMax;
+ else if (p > dMin)
+ return p;
+ else
+ return dMin;
}
+const float VKp = 1.1f;
+const float VKi = 0.01f;
+const float VKd = -0.000f;
+volatile float vPrevError = 0.0f;
+volatile float targetV = 300.0f;
+volatile float dutyParam = 0.0f;
+volatile float totalVErr = 0.0f;
+volatile float vPrevErr = 0.0f;
+volatile float vErr = 0;
void controlVelocity() {
- float vErr =
+ vErr = (targetV - w3) / targetV;
+ float dErr = vErr - vPrevErr;
+ totalVErr += vErr;
+ dutyParam = VKp*vErr + VKi*totalVErr + VKd*dErr;
+ duty = computeDelta(dutyParam);
+ if (vErr < 0) {
+// dutyParam = 0.1f;
+// lead = -lead;
+ }
+ vPrevErr = vErr;
+// dutyParam = 2.0f * VKp * vErr;
+// if (dutyParam > 0.0f) {
+// duty = min(1.0f, dutyParam / maxDutyParam);
+// } else {
+// printf("Reversed the motor!\n\r");
+// duty = min(1.0f, -dutyParam / maxDutyParam);
+// lead *= -1;
+// }
+// duty = 0.2f;
}
-void controlPosition(){
- if (w3 > 300) { //restrict the motor speed to 300
- lead = 2; // rad/s
- return;
- }
- prevError = currentError;
- currentRevs = diskPosition / 360 + count_i3; // angle/360 + #of revs
- currentError = goalRevs - currentRevs; //P term
-// if (currentError < 0.5f) {
-// printf("Reached final position: %f\n\r", currentRevs);
-// commandFinished = true;
-// return;
-// }
- dError = (currentError - prevError)/dtControl; //D term
- duty = k*(kp*currentError + kd*dError); //Control motor duty
- if (duty > 0) {
- lead = -2;
- } else { //if duty < 0, reverse motor spin
- lead = 2; // direction to decelerate
- duty = -duty;
- }
+void stopCommand() {
+ commandFinished = true;
+ lifeTicker.detach();
+ controlTicker.detach();
+}
+
+void setVelocity(float v) {
+ targetV = v;
+ goalRevs = 900.0f;
+ printf("Spinning with V=%.2f\n\r", targetV);
+ state = updateState();
+ motorOut((state-orState+lead+6)%6, 0.3f);
+ controlTicker.attach(&controlVelocity, dtControl);
+// motorSwitchOn();
+// motorOutTicker.attach(&runMotor, dtMotor);
+ lifeTicker.attach(&stopCommand, 10);
}
-void rotate(float r) {
- printf("Rotating for %f revolutions\n\r", r);
- goalRevs = r;
- state = updateState();
- // Kickstart
- motorOut((state-orState+lead+6)%6, 0.3f);
- controlTicker.attach(&controlPosition, dtControl);
- motorOutTicker.attach(&runMotor, dtMotor);
+void serialThread() {
+ while(true) {
+// if (readyForCommand) {
+// scanf("%s", userInput);
+// ParseResult curr = parse((char *) userInput);
+// if (curr.success) {
+// mutex.lock();
+// commandReady = true;
+// parseResult = curr;
+// mutex.unlock();
+// }
+// }
+// Thread::wait(10);
+// if (outputRequested) {
+ printf("V = %.2f; delta = %.2f dutyParam = %.2f vErr = %.2f vTotalErr= %.2f\n\r", w3, duty, dutyParam, vErr, totalVErr);
+// printf("Current pos: %.2f\n\r", diskPosition);
+ Thread::wait(2000);
+// printf("%s\n\r", userOutput);
+// outputRequested = false;
+// }
+ }
}
-
void i1rise(){
state = updateState();
- //motorOut((state-orState+lead+6)%6, duty);
+// motorOut((state-orState+lead+6)%6, duty);
- if (I3.read() == 1) { //Only count revolutions if the
- count_i3++; // rotor spins forward
- }
+ if (I3.read() == 1) //Only count revolutions if the
+ count_i3++; // rotor spins forward
}
//TODO merge with i_edge by measuring angular velocity in i1rise.
void i3rise(){
state = updateState();
-// motorOut((state-orState+lead+6)%6, duty);
-
- w3 = angle/dt_I3.read(); //Calc angular velocity
-
+// motorOut((state-orState+lead+6)%6, duty);
+ w3 = angle/dt_I3.read(); //Calc angular velocity
dt_I3.reset();
}
@@ -138,9 +198,10 @@
//Todo: add comments on this fucntion
void updateDiskPosition() {
if (CH_state != CH_state_prev) {
- int diff = CH_state - CH_state_prev;
-
+ float diff = CH_state - CH_state_prev;
+
CH_state_prev = CH_state;
+// diskPosition += -diff * angularResolution;
if (abs(diff) == 1 || abs(diff) == 3) {
if (diff < 0)
diskPosition += angularResolution;
@@ -187,13 +248,12 @@
updateDiskPosition();
}
-void runCommand(const ParseResult& command);
+int main() {
+ Thread serialHandler;
-int main() {
- Thread serialInput;
- serialInput.start(callback(serialThread));
+// motorControlThread.start(callback(runMotor));
motorHome(); //Initialise motor before any interrupt
-
+// stopMotor();
dt_I3.start(); //Start the time counters for velocity
@@ -209,42 +269,22 @@
CHB.rise(&CHB_rise);
CHB.fall(&CHB_fall);
-// state = updateState();
-// // Kickstart
-// motorOut((state-orState+lead+6)%6, 0.3f);
-// controlTicker.attach(&controlPosition, dtControl);
-// motorOutTicker.attach(&runMotor, dtMotor);
- printf("Ready\n\r");
+// printf("Ready\n\r");
+ serialHandler.start(callback(serialThread));
+ controlTicker.attach(&controlVelocity, dtControl);
+// setVelocity(100);
while (true) {
- if (count_i3 >= goalRevs && !commandFinished) commandFinished = true;
-// pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
-// printf("dError: %f , currentError: %f, prevError: %f, currentRevs: %f\n\r",dError, currentError, prevError, currentRevs);
- // Work with user input here
- if (commandReady) {
-// printf("Got command: %d\n\r", parseResult.mode);
- commandReady = false;
- readyForCommand = false;
- commandFinished = false;
- runCommand(parseResult);
- }
- if (commandFinished && !readyForCommand) {
- motorOutTicker.detach();
- stopMotor(); //Turn off the motor if position is reached
- readyForCommand = true;
- printf("Ready\n\r");
- }
+ motorOut((state-orState+lead+6)%6, duty);
+ // Values of 1 or 10 seem to make no difference for v = 100
+ Thread::wait(10);
+// printf("Current pos: %.2f\n\r", diskPosition);
}
return 0;
}
-void runCommand(const ParseResult& command) {
- int mode = command.mode;
- switch(mode) {
- case 0: rotateWith(command.rotations, command.velocity); break;
- case 1: rotate(command.rotations); break;
- case 2: setVelocity(command.velocity); break;
- case 3: playTunes(command.tunes); break;
- default: return;
- }
-}
+//
+//void monitor() {
+//// print("Current speed %.2f\n\r", w3);
+// print("Current revs: %.2f\n\r", currentRevs);
+//}
\ No newline at end of file
