Patrick Mabin
/
FPJA_ES_CW2
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.
Revision 40:093396961b3a, committed 2017-03-23
- Comitter:
- pcm13
- Date:
- Thu Mar 23 18:53:55 2017 +0000
- Branch:
- BaremetalSerial
- Parent:
- 37:7d0761ec1fd1
- Parent:
- 39:830888cceb3d
- Child:
- 41:53d9f9da1879
- Child:
- 42:8b6f9ce91605
- Commit message:
- Added better speed calculations and set number of rotations feedback
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp.orig | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Mar 23 14:58:29 2017 +0000
+++ b/main.cpp Thu Mar 23 18:53:55 2017 +0000
@@ -50,9 +50,9 @@
//PID feedback for constant speed
const double Rate = 0.5;//Define rate
-const double Propor = 20;//0.19;
-const double Deriv = 00000;
-const double Interg = 150;//0.35;
+const double Propor = 9;//7.5;//20;
+const double Deriv = 18;//9;
+const double Interg = 0;//150;
PID constspeed(Propor,Deriv,Interg,Rate);
@@ -128,6 +128,10 @@
//https://docs.mbed.com/docs/mbed-os-api-reference/en/5.1/APIs/io/InterruptIn/
+float speed = 0;
+volatile int turns = 0;
+volatile int count = 0;
+
class Position
{
public:
@@ -135,8 +139,7 @@
sensor1(pin_sensor1),
sensor2(pin_sensor2),
sensor3(pin_sensor3),
- count(0),
- turns(0) {
+ running(false) {
sensor1.rise(callback(this, &Position::sensor1_rise));
sensor2.rise(callback(this, &Position::sensor2_rise));
sensor3.rise(callback(this, &Position::sensor3_rise));
@@ -156,8 +159,10 @@
count++;
if(sensor2 == 0)turns++;
if(sensor2 == 1)turns--;
- if(lead == -2) motorOut((10-ofstate)%6);
- else motorOut((12-ofstate)%6);
+ if (running) {
+ if(lead == -2) motorOut((10-ofstate)%6);
+ else motorOut((12-ofstate)%6);
+ }
}
inline void sensor2_rise() {
@@ -173,8 +178,10 @@
}
inline void sensor1_fall() {
- if(lead == -2) motorOut((13-ofstate)%6);
- else motorOut((15-ofstate)%6);
+ if(running) {
+ if(lead == -2) motorOut((13-ofstate)%6);
+ else motorOut((15-ofstate)%6);
+ }
}
inline void sensor2_fall() {
@@ -186,34 +193,37 @@
if(lead == -2) motorOut((15-ofstate)%6);
else motorOut((11-ofstate)%6);
}
- inline void stop(){
- sensor1.disable_irq();
+ inline void stop() {
+ //sensor1.disable_irq();
+ running = false;
sensor2.disable_irq();
sensor3.disable_irq();
int8_t new_state = 33;
int8_t old_state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
- motorOut((old_state-ofstate+6)%6);
- while(new_state != old_state){
+ motorOut((old_state-ofstate+6)%6);
+ while(new_state != old_state) {
old_state = new_state;
new_state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
- motorOut((new_state-ofstate+6)%6);
+ motorOut((new_state-ofstate+6)%6);
//Max speed 50hz
wait_ms(12);
}
}
-
- inline void start(){
- sensor1.enable_irq();
+
+ inline void start() {
+ //sensor1.enable_irq();
+ running = true;
sensor2.enable_irq();
sensor3.enable_irq();
int8_t state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
motorOut((state-ofstate+lead+6)%6);
}
- volatile int count;
+ //volatile int count;
//Justify interrupt modifying this as atomic
- volatile int turns;
private:
+ bool running;
+
InterruptIn sensor1;
InterruptIn sensor2;
InterruptIn sensor3;
@@ -221,62 +231,36 @@
int Direction_I = 1;
+Position sensors(I1pin, I2pin, I3pin);
-void set_speed(double target_rotation)
+void set_speed(double new_rotation)
{
- if(target_rotation >0) {
- target_rotation = target_rotation*1.0;
+ if(new_rotation >0) {
+ new_rotation = new_rotation*1.0;
lead = -2;
Direction_I = 1;
} else {
lead = 2;
- target_rotation = target_rotation*-1.0;
+ new_rotation = new_rotation*-1.0;
Direction_I = -1;
}
- constspeed.setSetPoint(target_rotation);
+ constspeed.setSetPoint(new_rotation);
// constspeed.setBias(target_rotation/42);
}
//PID feedback for set distance
-const double Dist_Rate = 0.1;//Define rate
-const double Dist_Propor = 0.001;
-const double Dist_Deriv = 0;
-const double Dist_Interg = 0;
+const double Dist_Rate = 0.05;//Define rate
+const double Dist_Propor = 0;
+const double Dist_Deriv = 0.0000100;
+const double Dist_Interg = 000;
PID SetDist(Dist_Propor,Dist_Deriv,Dist_Interg,Dist_Rate);
double a;
+int current_distance;
+volatile int target_distance;
-void distance(int target_distance, int current_distance)
-{
- bool test = false;
- double new_speed;
- /*if((target_distance-current_distance)<1) {
- ///set_speed(0.0);
- L1L.write(0);
- L1H = 0;
- L2L.write(0);
- L2H = 0;
- L3L.write(0);
- L3H = 0;
- }*/
- if(target_distance < 0) {
- if((current_distance-target_distance)<100) test = true;
- }
- if(target_distance > 0) {
- if((target_distance-current_distance)<100) test = true;
- }
- if(test == true) {
- wait(Dist_Rate);
- SetDist.setSetPoint(target_distance);
- SetDist.setProcessValue(current_distance);
- new_speed = SetDist.compute();
- //new_speed = (target_distance - current_distance)*0.01;
- a = new_speed;
- set_speed(new_speed);
- }
-}
void feedback_setup()
{
@@ -291,7 +275,6 @@
}
-Mutex pid_rotation;
Mutex line_mutex;
volatile double rotation_time = 0;//The rotation in Hz as calculted using I1
@@ -300,23 +283,21 @@
void pid_thread_func()
{
while(true) {
- pid_rotation.lock();
+ if(lead == -2)constspeed.setProcessValue(rotation_time);
+ else constspeed.setProcessValue(-1.0*rotation_time);
+ pitch = constspeed.compute();
Thread::wait(Rate*1000);// Rate is in seconds from the PID
- constspeed.setProcessValue(rotation_time);
- pitch = constspeed.compute();
- pid_rotation.unlock();
}
}
-Thread pid_thread(osPriorityNormal, 1024, NULL);;
+
+
void Tx_interrupt();
void Rx_interrupt();
void send_line();
void read_line();
-
-
const int buffer_size = 255;
char tx_buffer[buffer_size+1];
@@ -333,7 +314,10 @@
volatile bool enable_buffers = true;
//Semaphore new_command();
-Thread set_commands(osPriorityNormal, 1024, NULL);
+Thread set_commands(osPriorityNormal, 512, NULL);
+Thread pid_thread(osPriorityNormal, 512, NULL);
+Thread distance_thread(osPriorityNormal, 512, NULL);
+
enum NOTES_NAMES { A, A_sharp, A_flat,
B, B_sharp, B_flat,
C, C_sharp, C_flat,
@@ -356,13 +340,13 @@
void set_commands_func()
{
- Thread *runnning_thread = NULL;
- Position sensors(I1pin, I2pin, I3pin);
+ Thread *running_thread = NULL;
+ Position sensors(I1pin, I2pin, I3pin);
char command[32];
while(true) {
Thread::signal_wait(0x1);
- if(running_thread != NULL){
- running_thread -> terminante();
+ if(running_thread != NULL) {
+ running_thread -> terminate();
}
sensors.stop();
read_line();
@@ -527,14 +511,15 @@
sprintf(tx_line, "Setting Revolutions %f\r\n ", n_rev);
send_line();
line_mutex.unlock();
- if(vel){ target_rotation = n_vel}
- distance_thread.start(distance);
+ if(vel)set_speed(n_vel);
+ else set_speed(25); //Default rotation speed
+ //distance_thread.start(distance);
sensors.start();
- running_thread = &distance;
+ running_thread = &distance_thread;
}
if(vel) {
- target_rotation = n_vel;
- sensors.start()
+ set_speed(n_vel);
+ sensors.start();
running_thread = NULL;
line_mutex.lock();
sprintf(tx_line, "Setting Velocity %f\r\n ", n_vel);
@@ -637,6 +622,50 @@
return;
}
+void distance()
+{
+ bool test = false;
+ double new_speed;
+ SetDist.setSetPoint(target_distance);
+ int dist_test;
+ int neg = 1;
+ if(target_distance<0) {
+ target_distance = target_distance*-1.0;
+ neg = -1;
+ }
+ while(true) {
+ dist_test = target_distance - turns*neg;
+ if(dist_test < 1 ) { //dist_test = dist_test*-1.0;
+ sensors.stop();
+ L1L.write(0);
+ L1H = 0;
+ L2L.write(0);
+ L2H = 0;
+ L3L.write(0);
+ L3H = 0;
+ }
+ /*line_mutex.lock();
+ sprintf(tx_line, "dist_test: %i\r\n", dist_test);
+ send_line();
+ line_mutex.unlock();*/
+ if(test == true) {
+ SetDist.setProcessValue(turns*neg);
+ new_speed = SetDist.compute();
+ //new_speed = (target_distance - turns)*0.1;
+ a = new_speed;
+ new_speed = 5;
+ set_speed(new_speed*neg);
+ Thread::wait(Dist_Rate*1000);
+ } else {
+ if(target_distance > 150 && dist_test < 100) test = true;
+ else if(dist_test < target_distance*(1.0/3.0)) test = true;
+ else {
+ //Keep rotating constant speed
+ Thread::wait(Dist_Rate*1000);
+ }
+ }
+ }
+}
//Main
int main()
{
@@ -650,10 +679,9 @@
pc.attach(&Tx_interrupt, Serial::TxIrq);
int8_t orState = 0; //Rotot offset at motor state 0
-
+ sensors.start();
line_mutex.lock();
sprintf(tx_line, "Hello\r\n");
- //printf("Hello\n\r");
send_line();
line_mutex.unlock();
@@ -675,45 +703,44 @@
t.start();
//char speed_time; //Input char Not Working
- int count = 0;//Count of number of rotations in given time
- int turns = 0;
int count2 =0;//Count for reporting at set invertals
- int countold =0;
+ int countold =turns;
+ target_distance = 100;
- double target_rotation = -20;// The set speed in Hz
+ double target_rotation = 20;// The set speed in Hz
- L1L.period_us(100);
- L2L.period_us(100);
- L3L.period_us(100);
-
+ L1L.period_us(10);
+ L2L.period_us(10);
+ L3L.period_us(10);
+
feedback_setup();
- set_commands.start(set_commands_func);
+ //set_commands.start(set_commands_func);
pid_thread.start(pid_thread_func);
-
- int target_distance = 200;
+ distance_thread.start(distance);
line_mutex.lock();
- sprintf(tx_line, "Start count %d\r\n", sensors.count);
+ sprintf(tx_line, "Start count %d\r\n",count);
send_line();
line_mutex.unlock();
-
+ set_speed(target_rotation);
+
while (1) {
// PID from https://developer.mbed.org/cookbook/PID
wait(1);
- count = sensors.count;//Counting the number of rotations
- turns = sensors.turns;
+ //count = sensors.count;//Counting the number of rotations
+ ///turns = sensors.turns;
//if(target_rotation <0)count = count*-1.0;
- pid_rotation.lock();
- rotation_time = (count-countold)/t.read();//calculating rotation from I1
- pid_rotation.unlock();
+ //pid_rotation.lock();
+ rotation_time = (turns-countold)/t.read();//calculating rotation from I1
+ //pid_rotation.unlock();
- countold = count;
+ countold = turns;
t.reset();
@@ -723,13 +750,12 @@
count = 0;
new_command = false;
}
- set_speed(target_rotation);
- //distance(target_distance, count);
+ //set_speed(target_rotation);
if (count2>0) { //report every so often as not to slow down loop
line_mutex.lock();
- sprintf(tx_line, "Rotation in Hz: %f %f %d %f\r\n",rotation_time*Direction_I , pitch , turns, a);
+ sprintf(tx_line, "Rotation in Hz: %f %f %d %f\r\n",rotation_time , pitch , turns, a);
send_line();
line_mutex.unlock();
count2 = 0;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig Thu Mar 23 18:53:55 2017 +0000
@@ -0,0 +1,740 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "PID.h"
+
+//Photointerrupter input pins
+#define I1pin D2
+#define I2pin D11
+#define I3pin D12
+
+//Incremental encoder input pins
+#define CHA D7
+#define CHB D8
+
+//Motor Drive output pins //Mask in output byte
+#define L1Lpin D4 //0x01
+#define L1Hpin D5 //0x02
+#define L2Lpin D3 //0x04
+#define L2Hpin D6 //0x08
+#define L3Lpin D9 //0x10
+#define L3Hpin D10 //0x20
+
+//Mapping from sequential drive states to motor phase outputs
+/*
+ State L1 L2 L3
+ 0 H - L
+ 1 - H L
+ 2 L H -
+ 3 L - H
+ 4 - L H
+ 5 H L -
+ 6 - - -
+ 7 - - -
+ */
+//Drive state to output table
+const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+
+//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
+const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
+//const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
+
+//Phase lead to make motor spin
+int8_t lead = -2; //2 for forwards, -2 for backwards
+
+const int8_t ofstate =4;
+
+
+//Initialise the serial port
+//https://developer.mbed.org/users/mbed_official/code/mbed-dev/file/default/targets/TARGET_STM/TARGET_STM32F3/TARGET_STM32F303x8/TARGET_NUCLEO_F303K8/PinNames.h
+RawSerial pc(SERIAL_TX, SERIAL_RX);
+
+//PID feedback for constant speed
+const double Rate = 0.5;//Define rate
+const double Propor = 20;//0.19;
+const double Deriv = 00000;
+const double Interg = 150;//0.35;
+
+PID constspeed(Propor,Deriv,Interg,Rate);
+
+//Status LED
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+//Photointerrupter inputs
+/*DigitalIn I1(I1pin);
+DigitalIn I2(I2pin);
+DigitalIn I3(I3pin);*/
+
+//Motor Drive outputs
+PwmOut L1L(L1Lpin);
+DigitalOut L1H(L1Hpin);
+PwmOut L2L(L2Lpin);
+DigitalOut L2H(L2Hpin);
+PwmOut L3L(L3Lpin);
+DigitalOut L3H(L3Hpin);
+
+//Setup flipper for changing rotor state
+Ticker flipper;
+
+int8_t intState = 0;
+int8_t intStateOld = 0;
+
+float pitch = 1;//Set for different speeds
+
+//Set a given drive state
+void motorOut(int8_t driveState)
+{
+
+ //Lookup the output byte from the drive state.
+ int8_t driveOut = driveTable[driveState & 0x07];
+
+ //Turn off first
+ if (~driveOut & 0x01) L1L.write(0);
+ if (~driveOut & 0x02) L1H = 1;
+ if (~driveOut & 0x04) L2L.write(0);
+ if (~driveOut & 0x08) L2H = 1;
+ if (~driveOut & 0x10) L3L.write(0);
+ if (~driveOut & 0x20) L3H = 1;
+
+ //Then turn on
+ if (driveOut & 0x01) L1L.write(pitch);
+ if (driveOut & 0x02) L1H = 0;
+ if (driveOut & 0x04) L2L.write(pitch);
+ if (driveOut & 0x08) L2H = 0;
+ if (driveOut & 0x10) L3L.write(pitch);
+ if (driveOut & 0x20) L3H = 0;
+}
+
+//Convert photointerrupter inputs to a rotor state
+inline int8_t readRotorState()
+{
+ //return stateMap[I1 + 2*I2 + 4*I3];
+}
+
+//Basic synchronisation routine
+int8_t motorHome()
+{
+ //Put the motor in drive state 0 and wait for it to stabilise
+ motorOut(0);
+ wait(1.0);
+
+ //Get the rotor state
+ return readRotorState();
+ // return sensors.readRotorState();
+}
+
+
+
+//https://docs.mbed.com/docs/mbed-os-api-reference/en/5.1/APIs/io/InterruptIn/
+
+class Position
+{
+public:
+ Position(PinName pin_sensor1, PinName pin_sensor2, PinName pin_sensor3) :
+ sensor1(pin_sensor1),
+ sensor2(pin_sensor2),
+ sensor3(pin_sensor3),
+ count(0),
+ turns(0) {
+ sensor1.rise(callback(this, &Position::sensor1_rise));
+ sensor2.rise(callback(this, &Position::sensor2_rise));
+ sensor3.rise(callback(this, &Position::sensor3_rise));
+ sensor1.fall(callback(this, &Position::sensor1_fall));
+ sensor2.fall(callback(this, &Position::sensor2_fall));
+ sensor3.fall(callback(this, &Position::sensor3_fall));
+ //start();
+ }
+
+ inline void update() {
+ int8_t state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
+ motorOut((state-ofstate+lead+6)%6);
+ }
+
+ //State could be set directly? Change of direction probably not
+ inline void sensor1_rise() {
+ count++;
+ if(sensor2 == 0)turns++;
+ if(sensor2 == 1)turns--;
+ if(lead == -2) motorOut((10-ofstate)%6);
+ else motorOut((12-ofstate)%6);
+ }
+
+ inline void sensor2_rise() {
+ if(lead == -2) motorOut((14-ofstate)%6);
+ else motorOut((10-ofstate)%6);
+ }
+
+ inline void sensor3_rise() {
+ //if(lead == -2) motorOut((13-ofstate)%6,1);
+ //else motorOut((10-ofstate)%6,1);
+ if(lead == -2) motorOut((12-ofstate)%6);
+ else motorOut((14-ofstate)%6);
+ }
+
+ inline void sensor1_fall() {
+ if(lead == -2) motorOut((13-ofstate)%6);
+ else motorOut((15-ofstate)%6);
+ }
+
+ inline void sensor2_fall() {
+ if(lead == -2) motorOut((11-ofstate)%6);
+ else motorOut((13-ofstate)%6);
+ }
+
+ inline void sensor3_fall() {
+ if(lead == -2) motorOut((15-ofstate)%6);
+ else motorOut((11-ofstate)%6);
+ }
+ inline void stop(){
+ sensor1.disable_irq();
+ sensor2.disable_irq();
+ sensor3.disable_irq();
+ int8_t new_state = 33;
+ int8_t old_state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
+ motorOut((old_state-ofstate+6)%6);
+ while(new_state != old_state){
+ old_state = new_state;
+ new_state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
+ motorOut((new_state-ofstate+6)%6);
+ //Max speed 50hz
+ wait_ms(12);
+ }
+ }
+
+ inline void start(){
+ sensor1.enable_irq();
+ sensor2.enable_irq();
+ sensor3.enable_irq();
+ int8_t state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
+ motorOut((state-ofstate+lead+6)%6);
+ }
+ volatile int count;
+ //Justify interrupt modifying this as atomic
+ volatile int turns;
+
+private:
+ InterruptIn sensor1;
+ InterruptIn sensor2;
+ InterruptIn sensor3;
+};
+
+
+int Direction_I = 1;
+
+void set_speed(double target_rotation)
+{
+ if(target_rotation >0) {
+ target_rotation = target_rotation*1.0;
+ lead = -2;
+ Direction_I = 1;
+
+ } else {
+ lead = 2;
+ target_rotation = target_rotation*-1.0;
+ Direction_I = -1;
+ }
+ constspeed.setSetPoint(target_rotation);
+// constspeed.setBias(target_rotation/42);
+}
+
+//PID feedback for set distance
+const double Dist_Rate = 0.1;//Define rate
+const double Dist_Propor = 0.001;
+const double Dist_Deriv = 0;
+const double Dist_Interg = 0;
+
+PID SetDist(Dist_Propor,Dist_Deriv,Dist_Interg,Dist_Rate);
+
+double a;
+
+void distance(int target_distance, int current_distance)
+{
+ bool test = false;
+ double new_speed;
+ /*if((target_distance-current_distance)<1) {
+ ///set_speed(0.0);
+ L1L.write(0);
+ L1H = 0;
+ L2L.write(0);
+ L2H = 0;
+ L3L.write(0);
+ L3H = 0;
+ }*/
+ if(target_distance < 0) {
+ if((current_distance-target_distance)<100) test = true;
+ }
+ if(target_distance > 0) {
+ if((target_distance-current_distance)<100) test = true;
+ }
+ if(test == true) {
+ wait(Dist_Rate);
+ SetDist.setSetPoint(target_distance);
+ SetDist.setProcessValue(current_distance);
+ new_speed = SetDist.compute();
+ //new_speed = (target_distance - current_distance)*0.01;
+ a = new_speed;
+ set_speed(new_speed);
+ }
+}
+
+void feedback_setup()
+{
+ //Feedback for speed controller
+ constspeed.setMode(1);
+ constspeed.setOutputLimits(0.00, 1.0);
+ constspeed.setInputLimits(-100.0, 100.0);
+
+ //Feedback for distance controller
+ SetDist.setMode(1);
+ SetDist.setOutputLimits(-50,50);
+}
+
+
+Mutex pid_rotation;
+Mutex line_mutex;
+
+volatile double rotation_time = 0;//The rotation in Hz as calculted using I1
+volatile float num = 0.0;
+
+void pid_thread_func()
+{
+ while(true) {
+ pid_rotation.lock();
+ Thread::wait(Rate*1000);// Rate is in seconds from the PID
+ constspeed.setProcessValue(rotation_time);
+ pitch = constspeed.compute();
+ pid_rotation.unlock();
+ }
+}
+
+Thread pid_thread(osPriorityNormal, 1024, NULL);;
+
+void Tx_interrupt();
+void Rx_interrupt();
+void send_line();
+void read_line();
+
+
+
+const int buffer_size = 255;
+
+char tx_buffer[buffer_size+1];
+char rx_buffer[buffer_size+1];
+
+volatile int tx_in=0;
+volatile int tx_out=0;
+volatile int rx_in=0;
+volatile int rx_out=0;
+
+char tx_line[80];
+char rx_line[80];
+
+volatile bool enable_buffers = true;
+
+//Semaphore new_command();
+Thread set_commands(osPriorityNormal, 1024, NULL);
+enum NOTES_NAMES { A, A_sharp, A_flat,
+ B, B_sharp, B_flat,
+ C, C_sharp, C_flat,
+ D, D_sharp, D_flat,
+ E, E_sharp, E_flat,
+ F, F_sharp, F_flat,
+ G, G_sharp, G_flat
+ };
+volatile bool vel = false; //command is stored into these variables
+volatile bool rev = false; //
+volatile double n_rev = 0; //
+volatile double n_vel = 0; //
+bool point = false;
+bool minus = false;
+bool song = false;
+volatile bool new_command = false;
+NOTES_NAMES notes [15];
+int durations [15];
+int notes_length;
+
+void set_commands_func()
+{
+ Thread *runnning_thread = NULL;
+ Position sensors(I1pin, I2pin, I3pin);
+ char command[32];
+ while(true) {
+ Thread::signal_wait(0x1);
+ if(running_thread != NULL){
+ running_thread -> terminante();
+ }
+ sensors.stop();
+ read_line();
+ sscanf(rx_line,"%s",command);
+ line_mutex.lock();
+ sprintf(tx_line, "processing new command: %s\r\n", command);
+ send_line();
+ line_mutex.unlock();
+ int length_command = 0;
+ while(command[length_command] != 0) {
+ //command[length_command] = rx_line[length_command];
+
+ line_mutex.lock();
+ sprintf(tx_line, " char: %c %i\r\n", command[length_command], command[length_command]);
+ send_line();
+ line_mutex.unlock();
+
+ length_command++;
+ }
+ line_mutex.lock();
+ sprintf(tx_line, "length: %i\r\n", length_command);
+ send_line();
+ line_mutex.unlock();
+
+
+ vel = false; //command is stored into these variables
+ rev = false; //
+ n_rev = 0; //
+ n_vel = 0; //
+ point = false;
+ minus = false;
+ song = false;
+ int i = 0;
+ if(command[i]=='T') {
+ song = true;
+ i++;
+ int j = 0;
+ while (i < length_command) {
+ bool sharp = false;
+ bool flat = false;
+ char note = command[i];
+ i++;
+ if (command[i]=='#') {
+ sharp=true;
+ i++;
+ }
+ if (command[i]=='^') {
+ flat=true;
+ i++;
+ }
+ switch (note) {
+ case 'A':
+ notes[j] = sharp ? A_sharp : (flat ? A_flat : A);
+ break;
+ case 'B':
+ notes[j] = sharp ? B_sharp : (flat ? B_flat : B);
+ break;
+ case 'C':
+ notes[j] = sharp ? C_sharp : (flat ? C_flat : C);
+ break;
+ case 'D':
+ notes[j] = sharp ? D_sharp : (flat ? D_flat : D);
+ break;
+ case 'E':
+ notes[j] = sharp ? E_sharp : (flat ? E_flat : E);
+ break;
+ case 'F':
+ notes[j] = sharp ? F_sharp : (flat ? F_flat : F);
+ break;
+ case 'G':
+ notes[j] = sharp ? G_sharp : (flat ? G_flat : G);
+ break;
+ default:
+ line_mutex.lock();
+ sprintf(tx_line, "Invalid note %c with int val %i\r\n ", note, note);
+ send_line();
+ line_mutex.unlock();
+ }
+ durations[j]=command[i]-48;
+ sprintf(tx_line, "Note %i\r\n ", note);
+ send_line();
+ sprintf(tx_line, "Val %i\r\n ", notes[j]);
+ send_line();
+ i++;
+ j++;
+ }
+ notes_length = j;
+ } else {
+ if(command[i]=='R') { //if first command is R
+ rev = true;
+ i++;
+ if (command[i]=='-') {
+ minus = true; //if command is negative
+ i++;
+ }
+ while (point==false && i<length_command && command[i] != 'V') { //nr of digits in the command
+ if (command[i]!='.') {
+ n_rev = (n_rev)*10 + (command[i]-48);
+ i++;
+ }
+ if (command[i]=='.') {
+ point=true;
+ float dec_place = 0.1;
+ while (i < length_command && command[i] != 'V') {
+ n_rev = n_rev + (command[i]-48)*dec_place ;
+ dec_place = dec_place*0.1;
+ i++;
+ }
+ }
+ }
+ line_mutex.lock();
+ sprintf(tx_line, "R %f\r\n ", n_rev);
+ send_line();
+ line_mutex.unlock();
+ } //R
+ if (i < length_command) {
+ //sprintf(tx_line, "command left\r\n");
+ //send_line();
+ if(command[i]=='V') { //if first (and therefore only) command is V
+ vel = true;
+ i++;
+
+ if (command[i]=='-') {
+ minus = true; //if command is negative
+ i++;
+ }
+ point = false;
+ while (point==false && i<length_command) { //nr of digits in the command
+ if (command[i]!='.') {
+ n_vel = (n_vel)*10 + (command[i]-48);
+ i++;
+ }
+ if (command[i]=='.') {
+ point=true;
+ float dec_place = 0.1;
+ for (int j = (i+1); j<length_command; j++) {
+ n_vel = n_vel + (command[i]-48)*dec_place ;
+ dec_place = dec_place*0.1;
+ }
+ }
+ }
+ } else {
+ line_mutex.lock();
+ sprintf(tx_line, "Invalid command syntax\r\n");
+ send_line();
+ line_mutex.unlock();
+ }
+ }
+ }
+ if (song) {
+ sprintf(tx_line, "Read song command\r\n");
+ send_line();
+ for (int a = 0; a < notes_length; a++) {
+ line_mutex.lock();
+ sprintf(tx_line, "Note: %i Duration: %i\r\n", notes[a], durations[a]);
+ send_line();
+ line_mutex.unlock();
+ }
+ } else {
+ if(rev) {
+ line_mutex.lock();
+ sprintf(tx_line, "Setting Revolutions %f\r\n ", n_rev);
+ send_line();
+ line_mutex.unlock();
+ if(vel){ target_rotation = n_vel}
+ distance_thread.start(distance);
+ sensors.start();
+ running_thread = &distance;
+ }
+ if(vel) {
+ target_rotation = n_vel;
+ sensors.start()
+ running_thread = NULL;
+ line_mutex.lock();
+ sprintf(tx_line, "Setting Velocity %f\r\n ", n_vel);
+ send_line();
+ line_mutex.unlock();
+ }
+ }
+ }
+ new_command = true;
+}
+
+
+
+
+// Copy tx line buffer to large tx buffer for tx interrupt routine
+void send_line()
+{
+ int i;
+ char temp_char;
+ bool empty;
+ i = 0;
+ NVIC_DisableIRQ(USART2_IRQn);
+ empty = (tx_in == tx_out);
+ //led3 = 1;
+ while ((i==0) || (tx_line[i-1] != '\n')) {
+ if (((tx_in + 1) % buffer_size) == tx_out) {
+ NVIC_EnableIRQ(USART2_IRQn);
+ while (((tx_in + 1) % buffer_size) == tx_out) {
+ }
+ NVIC_DisableIRQ(USART2_IRQn);
+ }
+ // Thread::wait(1000);
+ // led3 = 0;
+ //Thread::wait(1000);
+ // led3 = 1;
+ tx_buffer[tx_in] = tx_line[i];
+ i++;
+ tx_in = (tx_in + 1) % buffer_size;
+ }
+ if (pc.writeable() && (empty)) {
+ temp_char = tx_buffer[tx_out];
+ tx_out = (tx_out + 1) % buffer_size;
+ pc.putc(temp_char);
+ //pc.putc(temp_char);
+ }
+ NVIC_EnableIRQ(USART2_IRQn);
+ return;
+}
+
+
+// Read a line from the large rx buffer from rx interrupt routine
+void read_line()
+{
+ int i;
+ i = 0;
+ //led3 = 0;
+ NVIC_DisableIRQ(USART2_IRQn);
+ while ((i==0) || (rx_line[i-1] != '\r')) {
+ if (rx_in == rx_out) {
+ NVIC_EnableIRQ(USART2_IRQn);
+ while (rx_in == rx_out) {
+ }
+ NVIC_DisableIRQ(USART2_IRQn);
+ }
+ rx_line[i] = rx_buffer[rx_out];
+ i++;
+ rx_out = (rx_out + 1) % buffer_size;
+ }
+ NVIC_EnableIRQ(USART2_IRQn);
+ rx_line[i-1] = 0;
+ return;
+}
+
+
+// Interupt Routine to read in data from serial port
+void Rx_interrupt()
+{
+ char c;
+ while ((pc.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
+ c = pc.getc();
+ pc.putc(c);
+ rx_buffer[rx_in] = c;
+ rx_in = (rx_in + 1) % buffer_size;
+ if(c == '\r') {
+ set_commands.signal_set(0x1);
+ //led3=1;
+ }
+ }
+ return;
+}
+
+
+void Tx_interrupt()
+{
+ //led3 = 1;
+ while ((pc.writeable()) && (tx_in != tx_out)) {
+ pc.putc(tx_buffer[tx_out]);
+ tx_out = (tx_out + 1) % buffer_size;
+ }
+ return;
+}
+
+//Main
+int main()
+{
+ led3 = 0;
+ int rx_i=0;
+ pc.baud(9600);
+
+// Setup a serial interrupt function to receive data
+ pc.attach(&Rx_interrupt, Serial::RxIrq);
+// Setup a serial interrupt function to transmit data
+ pc.attach(&Tx_interrupt, Serial::TxIrq);
+
+ int8_t orState = 0; //Rotot offset at motor state 0
+
+ line_mutex.lock();
+ sprintf(tx_line, "Hello\r\n");
+ //printf("Hello\n\r");
+ send_line();
+ line_mutex.unlock();
+
+
+ //Run the motor synchronisation
+ //orState = motorHome();
+ line_mutex.lock();
+ sprintf(tx_line, "Rotor origin: %x\r\n",orState);
+ send_line();
+ line_mutex.unlock();
+ wait(0.5);
+
+ //orState is subtracted from future rotor state inputs to align rotor and motor states
+
+ //setting up limits on PID
+ //constspeed.setOutputLimits(0.0, 0.5);
+
+ Timer t;
+ t.start();
+
+ //char speed_time; //Input char Not Working
+ int count = 0;//Count of number of rotations in given time
+ int turns = 0;
+ int count2 =0;//Count for reporting at set invertals
+ int countold =0;
+
+ double target_rotation = -20;// The set speed in Hz
+
+
+ L1L.period_us(100);
+ L2L.period_us(100);
+ L3L.period_us(100);
+
+
+ feedback_setup();
+
+ set_commands.start(set_commands_func);
+ pid_thread.start(pid_thread_func);
+
+ int target_distance = 200;
+
+
+ line_mutex.lock();
+ sprintf(tx_line, "Start count %d\r\n", sensors.count);
+ send_line();
+ line_mutex.unlock();
+
+
+ while (1) {
+ // PID from https://developer.mbed.org/cookbook/PID
+ wait(1);
+
+ count = sensors.count;//Counting the number of rotations
+ turns = sensors.turns;
+ //if(target_rotation <0)count = count*-1.0;
+ pid_rotation.lock();
+ rotation_time = (count-countold)/t.read();//calculating rotation from I1
+ pid_rotation.unlock();
+
+ countold = count;
+ t.reset();
+
+
+ if(new_command) {
+ target_rotation = n_vel;
+ target_distance = n_rev;
+ count = 0;
+ new_command = false;
+ }
+ set_speed(target_rotation);
+ //distance(target_distance, count);
+
+
+ if (count2>0) { //report every so often as not to slow down loop
+ line_mutex.lock();
+ sprintf(tx_line, "Rotation in Hz: %f %f %d %f\r\n",rotation_time*Direction_I , pitch , turns, a);
+ send_line();
+ line_mutex.unlock();
+ count2 = 0;
+ }
+ count2++;
+
+ } //while(1)
+} //main
