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:
- 48:02d5565b8fac
- Parent:
- 47:49e6c3fb25dc
- Child:
- 49:731c95cd5852
--- a/main.cpp Fri Mar 24 14:48:27 2017 +0000
+++ b/main.cpp Fri Mar 24 16:23:37 2017 +0000
@@ -1,37 +1,35 @@
-#include <cmath>
+//#include <cmath>
#include <vector>
-//#include "mbed.h"
-//#include "rtos.h"
#include "definitions.h"
#include "motorControl.h"
//#include "parser.h"
//#include "serialHandler.h"
-//Mutex mutex;
-//Thread serialOutputer;
+Mutex mutex;
+Thread serialOutputer;
//
//Thread serialInputer;
#define print(...) sprintf((char*)userOutput, __VA_ARGS__); outputRequested = true;
//ParseResult parseResult;
-//volatile char userInput[256];
-//volatile char userOutput[256];
-//volatile bool outputRequested=false;
-//
-//volatile bool commandReady = false;
-//volatile bool readyForCommand = true;
-//void serialOut() {
-// while(true) {
-// if (outputRequested) {
-// printf("%s\n\r", userOutput);
-// mutex.lock();
-// outputRequested = false;
-// mutex.unlock();
-// }
-// }
-//}
+volatile char userInput[256];
+volatile char userOutput[256];
+volatile bool outputRequested=false;
+
+volatile bool commandReady = false;
+volatile bool readyForCommand = true;
+void serialOut() {
+ while(true) {
+ if (outputRequested) {
+ printf("%s\n\r", userOutput);
+ mutex.lock();
+ outputRequested = false;
+ mutex.unlock();
+ }
+ }
+}
//void serialIn() {
// while(true) {
@@ -52,12 +50,12 @@
volatile float w3 = 0; //Angular velocity
volatile int count_i3 = 0;
-volatile int CHA_state = 0x00;
-volatile int CHB_state = 0x00;
-volatile int CH_state = 0x00;
-volatile int CH_state_prev = 0x00;
+//volatile int CHA_state = 0x00;
+//volatile int CHB_state = 0x00;
+//volatile int CH_state = 0x00;
+//volatile int CH_state_prev = 0x00;
-volatile float diskPosition = 0.0; //in degrees
+//volatile float diskPosition = 0.0; //in degrees
volatile float velocityDuty = 0.0;
Timer dt_I3;
@@ -119,20 +117,22 @@
// velocityTon = velocityPeriod * velocityDuty;
//}
-const float VKp = 0.9f; //0.054, 1.1, 0.012
-const float VKi = 0.001f; //0.0054, 0.01
-const float VKd = 0.05f; //0.0135, 0.02
+const float VKp = 0.23f; //0.054, 1.1, 0.012
+const float VKi = 0.0085f; //0.0054, 0.01
+const float VKd = 0.02f; //0.0135, 0.02, 0.012
-volatile float targetV = 300.0f;
+volatile float targetV = 150.0f;
volatile float totalVErr = 0.0f;
volatile float vPrevErr = 0.0f;
volatile float vErr = 0.0f;
-volatile float dErr = 0.0f;
+volatile double dErr = 0.0f;
const float dtControl = 0.01f;
//Thread::wait(0.1) in the main for calling motorOut
void controlVelocity(){
+ if(w3 > targetV*2)
+ w3 = targetV;
vErr = targetV - w3;
dErr = (vErr - vPrevErr)/dtControl;
totalVErr += vErr*dtControl;
@@ -150,11 +150,11 @@
//volatile float profilerDt = 0;
//takes 11-12us
-inline void i3rise(){
+void i3rise(){
// profiler.reset();
// profiler.start();
state = updateState();
- motorOut((state-orState+lead+6)%6, velocityDuty);
+// motorOut((state-orState+lead+6)%6, velocityDuty);
w3 = angle/dt_I3.read(); //Calc angular velocity
dt_I3.reset();
@@ -166,60 +166,60 @@
void i_edge(){
state = updateState();
- motorOut((state-orState+lead+6)%6, velocityDuty);
+// motorOut((state-orState+lead+6)%6, velocityDuty);
}
-void updateDiskPosition() {
- if (CH_state != CH_state_prev) {
- int diff = CH_state - CH_state_prev;
-
- CH_state_prev = CH_state;
- if (abs(diff) == 1 || abs(diff) == 3) {
- if (diff < 0)
- diskPosition += angularResolution;
- else
- diskPosition -= angularResolution;
- }
- else if (abs(diff) == 2) {
- if (diff < 0)
- diskPosition += 2.0f * angularResolution;
- else
- diskPosition -= 2.0f * angularResolution;
- }
-
- if (diskPosition >= 360.0f) {
- diskPosition -= 360.0f;
- } else if (diskPosition < -360.0f) {
- diskPosition += 360.0f;
- }
- }
-}
+//void updateDiskPosition() {
+// if (CH_state != CH_state_prev) {
+// int diff = CH_state - CH_state_prev;
+//
+// CH_state_prev = CH_state;
+// if (abs(diff) == 1 || abs(diff) == 3) {
+// if (diff < 0)
+// diskPosition += angularResolution;
+// else
+// diskPosition -= angularResolution;
+// }
+// else if (abs(diff) == 2) {
+// if (diff < 0)
+// diskPosition += 2.0f * angularResolution;
+// else
+// diskPosition -= 2.0f * angularResolution;
+// }
+//
+// if (diskPosition >= 360.0f) {
+// diskPosition -= 360.0f;
+// } else if (diskPosition < -360.0f) {
+// diskPosition += 360.0f;
+// }
+// }
+//}
-void updateRelativeState() {
- CH_state = relativeStateMap[CHB_state + 2*CHA_state];
-}
-
-inline void CHA_rise() {
- CHA_state = 1;
- updateRelativeState();
- updateDiskPosition();
-}
-// Takes 5-6us
-inline void CHA_fall() {
- CHA_state = 0;
- updateRelativeState();
- updateDiskPosition();
-}
-inline void CHB_rise() {
- CHB_state = 1;
- updateRelativeState();
- updateDiskPosition();
-}
-inline void CHB_fall() {
- CHB_state = 0;
- updateRelativeState();
- updateDiskPosition();
-}
+//void updateRelativeState() {
+// CH_state = relativeStateMap[CHB_state + 2*CHA_state];
+//}
+//
+//inline void CHA_rise() {
+// CHA_state = 1;
+// updateRelativeState();
+// updateDiskPosition();
+//}
+//// Takes 5-6us
+//inline void CHA_fall() {
+// CHA_state = 0;
+// updateRelativeState();
+// updateDiskPosition();
+//}
+//inline void CHB_rise() {
+// CHB_state = 1;
+// updateRelativeState();
+// updateDiskPosition();
+//}
+//inline void CHB_fall() {
+// CHB_state = 0;
+// updateRelativeState();
+// updateDiskPosition();
+//}
//inline void motorControl(){
// if(pwmOut == 1) //Only control
@@ -241,13 +241,6 @@
//}
int main() {
- I1.rise(&i_edge); //Assign interrupt handlers for LEDs
- I1.fall(&i_edge);
- I2.rise(&i_edge);
- I2.fall(&i_edge);
- I3.rise(&i3rise);
- I3.fall(&i_edge);
-
// LPins[0] = &L1L; //Define the pins for the pin array
// LPins[1] = &L1H;
// LPins[2] = &L2L;
@@ -256,18 +249,29 @@
// LPins[5] = &L3H;
motorHome();
+
+ I1.rise(&i_edge); //Assign interrupt handlers for LEDs
+ I1.fall(&i_edge);
+ I2.rise(&i_edge);
+ I2.fall(&i_edge);
+ I3.rise(&i3rise);
+ I3.fall(&i_edge);
dt_I3.start(); //Start the time count for velocity
// controlTicker.attach(&control, dt);
// motorTicker.attach(&motorControl, dtMotor); //Call motor control periodicly
- CHA.rise(&CHA_rise);
- CHA.fall(&CHA_fall);
- CHB.rise(&CHB_rise);
- CHB.fall(&CHB_fall);
+// CHA.rise(&CHA_rise);
+// CHA.fall(&CHA_fall);
+// CHB.rise(&CHB_rise);
+// CHB.fall(&CHB_fall);
- motorOut(4, 0.4f); //Kickstart the motor
- controlTicker.attach(&controlVelocity, dtControl);
+ controlTicker.attach(&controlVelocity, dtControl);
+// printTicker.attach(&interruptPrint, 1.0f);
+ state = updateState();
+ motorOut((state-orState+lead+6)%6, 1.0f); //Kickstart the motor
+ wait(0.1);
+
// precisionMotorOut(360-120); //Kickstart motor with 120deg
// pwmPeriod.attach(&velocityPwmPeriod, velocityPeriod);
@@ -277,10 +281,11 @@
// serialInputer.set_priority(osPriorityLow);
while (true) {
-// Thread::wait(1000);
-// motorOut((state-orState+lead+6)%6, velocityDuty);
- pc.printf("Speed: %f, duty cycle: %f, currentRevs: %i, vErr %f, dErr %f, totalVErr: %f \n\r",w3, velocityDuty, count_i3, vErr, dErr, totalVErr);
- wait(0.1);
+ Thread::wait(1);
+ motorOut((state-orState+lead+6)%6, velocityDuty);
+ print("Speed: %f, duty cycle: %f, currentRevs: %i, vErr %f, prevErr; %f, dErr %f, totalVErr: %f \n\r",w3, velocityDuty, count_i3, VKp*vErr,VKp*vPrevErr, dErr, VKi*totalVErr);
+
+// wait(0.1);
// if(currentRevs >= goalRevs){
// controlTicker.detach();
//// pwmPeriod.detach();
