Sven Kugathasan
/
SKAFMO_2
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:
- 18:19d35daed140
- Parent:
- 17:209ac0b10ba1
- Child:
- 19:d79692cef6c7
--- a/main.cpp Fri Mar 24 16:29:01 2017 +0000
+++ b/main.cpp Fri Mar 24 19:31:07 2017 +0000
@@ -8,7 +8,7 @@
#include <string>
#include "stdlib.h"
#include "math.h"
-
+#include <limits>
/*_________________________________PIN SETUP__________________________________*/
@@ -29,6 +29,7 @@
#define L3Lpin D9 //0x10
#define L3Hpin D10 //0x20
+
//Photointerrupter Inputs as Interrupts
InterruptIn InterruptI1(D2);
InterruptIn InterruptI2(D11);
@@ -62,8 +63,8 @@
Timer tmp; // Profiler Timer
//PID Controller
-PID velocity_pid(0.35, 0.35, 0.35, 0.01); // (P, I, D, WAIT)
-PID dist_pid(10, 0.0, 0.01, 0.01); // (P, I, D, WAIT)
+PID velocity_pid(0.75, 0.025, 0.2, 0.1); // (P, I, D, WAIT)
+PID dist_pid(10, 0.0, 0.01, 0.1); // (P, I, D, WAIT)
//Initialize Threads
Thread pid_thread(osPriorityNormal, 512, NULL);
@@ -114,9 +115,7 @@
float angular_vel = 0.0f; //Revolution per second (Measured over 360)
float partial_vel = 0.0f; //Revolution per second (Measured over 360/117)
-float drive_vel = 0.0f;
float vel_target = 0.0f; //Target Speed
-
float vel_max = 100; //Maximum Speed at 3.0V achievable is ~60 rps
//Position Variables
@@ -133,14 +132,9 @@
bool flag = false;
float test_time = 0.0f;
int8_t test = 0;
-float a;
-float b;
-typedef struct{
- float a;
- float b;
-}float2;
-
+// Timer interrupt
+Ticker calcPID_timer;
/*_____Basic Functions (Motor Drive, Synchronization, Reading Rotor State)____*/
@@ -167,12 +161,26 @@
if (driveOut & 0x20) L3H = 0;
}
-
+
//Convert photointerrupter inputs to a rotor state
inline int8_t readRotorState(){
return stateMap[InterruptI1.read() + 2*InterruptI2.read() + 4*InterruptI3.read()];
}
+void calculatePID(){
+ TIME = 1;
+ //Calculate new PID Control Point
+ if(0 && (total_rev/rev_target) > 0.75f){
+ dist_pid.setProcessValue(total_rev);
+ dutyout = dist_pid.compute();
+ }
+ else{
+ velocity_pid.setProcessValue(partial_vel);
+ dutyout = velocity_pid.compute();
+ }
+ TIME =0;
+}
+
//Basic synchronisation routine
int8_t motorHome() {
//Put the motor in drive state X (e.g. 5) to avoid initial jitter
@@ -192,45 +200,56 @@
/*________________Advanced Functions (Speed and Position Control)_____________*/
// Function involves PID
-void position_control(float2 *cmd){
+void position_control(float rev, float vel){
- rev_target = cmd->a;
- vel_target = cmd->b;
+ completed = 0;
+ dutyout = 1.0f;
+ count = 0;
+
+ rev_target = rev;
+ vel_target = vel;
+ pc.printf("rev %f\r\n", rev);
+ pc.printf("vel %f\r\n", vel);
//Reverses motor direction if forwards rotation requested
- if((rev_target < 0)){
- direction = -1;
- rev_target = rev_target * -1;
+ if (rev_target < 0.0f || vel_target < 0.0f){
+ direction = -1;
+ if (rev_target < 0.0f)
+ rev_target = rev_target * -1;
+ else
+ vel_target = vel_target * -1;
}
- else if(vel_target < 0){
- direction = -1;
- vel_target = vel_target * -1;
+ else{
+ direction = 1;
}
- velocity_pid.setInputLimits(0.0, 2*vel_target);
+ pc.printf("vel_target %f\r\n", vel_target);
+ pc.printf("dir %d\r\n", direction);
+
+ pc.printf("Waiting for stabilize... %d\r\n", direction);
+ dutyout = 0.0f;
+ wait(3);
+ motorHome();
+ pc.printf("Restarting... %d\r\n", direction);
+
+ velocity_pid.reset();
+ velocity_pid.setInputLimits(0.0, 50);
velocity_pid.setOutputLimits(0.0, 1.0);
velocity_pid.setMode(1);
velocity_pid.setSetPoint(vel_target);
+ dist_pid.reset();
dist_pid.setInputLimits(0.0, rev_target);
dist_pid.setOutputLimits(0.0, 1.0);
dist_pid.setMode(1);
dist_pid.setSetPoint(rev_target);
-
+
+ dutyout = 0.3f;
intState = readRotorState();
driveto = (intState-orState+(direction*lead)+6)%6;
motorOut(driveto);
- while(!completed){
- pc.printf("dutyout: %f \r\n", dutyout);
- //pc.printf("Error: %f \r\n", (rev_target - total_rev));
- //pc.printf("DutyA: %f \r\n", a);
- //pc.printf("DutyB: %f \r\n", b);
- //pc.printf("\n");
-
- }
- pc.printf("Thread finished \r\n");
-
+ calcPID_timer.attach(&calculatePID, 0.1);
}
void changestate_isr(){
@@ -269,13 +288,20 @@
// Measure number of revolutions
count++;
+ if (rev_target == std::numeric_limits<float>::max()){
+ total_rev = 0.0f;
+ }
+
//Turn-off when target reached (if target is 0, never end)
- if( rev_target && total_rev >= rev_target){
+ if(completed || total_rev >= rev_target){
completed = 1;
+ total_rev = 0;
+ count = 0;
dutyout = 0;
motorOut(0);
led3 = 0;
- __disable_irq();
+ partial_rev = 0;
+ calcPID_timer.detach();
}
else{
intState = readRotorState();
@@ -307,15 +333,6 @@
//Total Revolution Count
total_rev = (count/6.0f) + partial_rev;
- //Calculate new PID Control Point
- if((total_rev/rev_target) > 0.75f){
- dist_pid.setProcessValue(total_rev);
- dutyout = dist_pid.compute();
- }
- else{
- velocity_pid.setProcessValue(partial_vel);
- dutyout = velocity_pid.compute();
- }
}
/*__________________________Main Function_____________________________________*/
@@ -327,19 +344,18 @@
float v=0; //velocity
bool r_val=true;
bool v_val=true;
- int t_loc=0;
- int r_loc=0;
- int v_loc=0;
- char buf[80];
+ int16_t t_loc=0;
+ int16_t r_loc=0;
+ int16_t v_loc=0;
+ char buf[20];
bool note_marker;
bool dur_marker;
bool accent_marker;
string note="";
- int duration=0;
+ uint8_t duration=0;
string input;
- float2 cmd_set;
while(1){
r=0;
v=0;
@@ -411,16 +427,8 @@
if(r_val==true){
r=atof(input.substr(1).c_str());
- cmd_set = (float2){r,vel_max};
- if (pid_thread.get_state() == Thread::Running){
- pc.printf("Thread running...Terminating \r\n");
- pid_thread.terminate();
- dutyout = 0.0;
- }
pc.printf("Spin for %.3f number of rotations\r\n",r);
- dutyout = 1.0;
- pid_thread.start(callback(position_control, &cmd_set));
- pid_thread.join();
+ position_control((float) r, vel_max);
}
else{
pc.printf("Invalid input\r\n");
@@ -446,18 +454,9 @@
if(v<0){
v=abs(v);
}
+ pc.printf("Spin for %.3f number of rotations at %.3f speed \r\n",r,v);
- cmd_set = (float2){r,v};
- if (pid_thread.get_state() == Thread::Running){
- pc.printf("Thread running...Terminating \r\n");
- pid_thread.terminate();
- dutyout = 0.0;
- }
- pc.printf("Spin for %.3f number of rotations at %.3f speed \r\n",r,v);
- dutyout = 1.0;
- pid_thread.start(callback(position_control, &cmd_set));
- pid_thread.join();
- pc.printf("Thread done. \r\n");
+ position_control((float) r, (float) v);
}
else{
pc.printf("Invalid input\r\n");
@@ -472,17 +471,12 @@
}
if(v_val==true){
v=atof(input.substr(1).c_str());
-
- cmd_set = (float2){revstates_max,v};
- if (pid_thread.get_state() == Thread::Running){
- pc.printf("Thread running...Terminating \r\n");
- pid_thread.terminate();
- dutyout = 0.0;
+ pc.printf("Spin at %.3f speed\r\n",v);
+ position_control(std::numeric_limits<float>::max(),(float)v);
+ while(!completed){
+ pc.printf("Duty Cycle: %.3f\r\n partial_val: %.3f\r\n", dutyout, partial_vel);
+ wait(1);
}
- pc.printf("Spin at %.3f speed\r\n",v);
- dutyout = 1.0;
- pid_thread.start(callback(position_control, &cmd_set));
- pid_thread.join();
}
else{
pc.printf("Invalid input\r\n");
@@ -530,7 +524,6 @@
//pid_thread.join();
- dutyout = 1.0;
serial_com();
}
