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 41:53d9f9da1879, committed 2017-03-24
- Comitter:
- JesusE
- Date:
- Fri Mar 24 10:56:24 2017 +0000
- Branch:
- BaremetalSerial
- Parent:
- 40:093396961b3a
- Child:
- 43:8384b26122b8
- Commit message:
- Thread system;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Mar 23 18:53:55 2017 +0000
+++ b/main.cpp Fri Mar 24 10:56:24 2017 +0000
@@ -39,9 +39,9 @@
//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
+int8_t lead = 2; //2 for forwards, -2 for backwards
-const int8_t ofstate =4;
+volatile int8_t ofstate =4;
//Initialise the serial port
@@ -80,7 +80,27 @@
int8_t intState = 0;
int8_t intStateOld = 0;
-float pitch = 1;//Set for different speeds
+float pwm_duty = 1;//Set for different speeds
+
+
+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];
+
//Set a given drive state
void motorOut(int8_t driveState)
@@ -98,11 +118,11 @@
if (~driveOut & 0x20) L3H = 1;
//Then turn on
- if (driveOut & 0x01) L1L.write(pitch);
+ if (driveOut & 0x01) L1L.write(pwm_duty);
if (driveOut & 0x02) L1H = 0;
- if (driveOut & 0x04) L2L.write(pitch);
+ if (driveOut & 0x04) L2L.write(pwm_duty);
if (driveOut & 0x08) L2H = 0;
- if (driveOut & 0x10) L3L.write(pitch);
+ if (driveOut & 0x10) L3L.write(pwm_duty);
if (driveOut & 0x20) L3H = 0;
}
@@ -112,6 +132,8 @@
//return stateMap[I1 + 2*I2 + 4*I3];
}
+void distance();
+
//Basic synchronisation routine
int8_t motorHome()
{
@@ -128,30 +150,37 @@
//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;
+Mutex line_mutex;
class Position
{
public:
Position(PinName pin_sensor1, PinName pin_sensor2, PinName pin_sensor3) :
+ running(false),
sensor1(pin_sensor1),
sensor2(pin_sensor2),
- sensor3(pin_sensor3),
- running(false) {
+ sensor3(pin_sensor3) {
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));
+ sensor2.disable_irq();
+ sensor3.disable_irq();
//start();
}
- inline void update() {
- int8_t state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
- motorOut((state-ofstate+lead+6)%6);
+ int8_t motor_home() {
+
+ motorOut(0);
+ wait(5);
+ led3=1;
+ int8_t home_state=stateMap[sensor1 + 2*sensor2 + 4*sensor3];
+ motorOut(3);
+ return home_state;
}
//State could be set directly? Change of direction probably not
@@ -171,8 +200,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);
}
@@ -198,16 +225,25 @@
running = false;
sensor2.disable_irq();
sensor3.disable_irq();
- int8_t new_state = 33;
+ //int8_t new_state = 33;
int8_t old_state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
motorOut((old_state-ofstate+6)%6);
+ wait(4);
+ /*L1L.write(0);
+ L1H = 0;
+ L2L.write(0);
+ L2H = 0;
+ L3L.write(0);
+ L3H = 0;*/
+ /*
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-lead+6)%6);
//Max speed 50hz
- wait_ms(12);
+ wait_ms(20);
}
+ */
}
inline void start() {
@@ -217,9 +253,13 @@
sensor3.enable_irq();
int8_t state = stateMap[sensor1 + 2*sensor2 + 4*sensor3];
motorOut((state-ofstate+lead+6)%6);
+ /*
+ line_mutex.lock();
+ sprintf(tx_line, "Start at: %i to %i lead %i %i\r\n", state, (state-ofstate+lead+6)%6, lead, ofstate);
+ send_line();
+ line_mutex.unlock();
+ */
}
- //volatile int count;
- //Justify interrupt modifying this as atomic
private:
bool running;
@@ -275,7 +315,7 @@
}
-Mutex line_mutex;
+
volatile double rotation_time = 0;//The rotation in Hz as calculted using I1
volatile float num = 0.0;
@@ -285,7 +325,7 @@
while(true) {
if(lead == -2)constspeed.setProcessValue(rotation_time);
else constspeed.setProcessValue(-1.0*rotation_time);
- pitch = constspeed.compute();
+ pwm_duty = constspeed.compute();
Thread::wait(Rate*1000);// Rate is in seconds from the PID
}
}
@@ -293,30 +333,12 @@
-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, 512, NULL);
-Thread pid_thread(osPriorityNormal, 512, NULL);
-Thread distance_thread(osPriorityNormal, 512, NULL);
+Thread set_commands(osPriorityNormal, 1024, NULL);
+Thread pid_thread(osPriorityNormal, 340, NULL);
+Thread song_thread(osPriorityNormal, 512, NULL);
+Thread distance_thread(osPriorityNormal, 340, NULL);
enum NOTES_NAMES { A, A_sharp, A_flat,
B, B_sharp, B_flat,
@@ -333,22 +355,28 @@
bool point = false;
bool minus = false;
bool song = false;
-volatile bool new_command = false;
+volatile bool output_main = false;
NOTES_NAMES notes [15];
int durations [15];
int notes_length;
+bool run_dist = false;
+bool run_song = false;
+
void set_commands_func()
{
- Thread *running_thread = NULL;
- Position sensors(I1pin, I2pin, I3pin);
char command[32];
while(true) {
Thread::signal_wait(0x1);
- if(running_thread != NULL) {
- running_thread -> terminate();
- }
+ run_dist = false;
+ run_song = false;
+ turns = 0;
+ count = 0;
sensors.stop();
+ //Thread::wait(2000);
+ L1L.period_us(10);
+ L2L.period_us(10);
+ L3L.period_us(10);
read_line();
sscanf(rx_line,"%s",command);
line_mutex.lock();
@@ -357,13 +385,10 @@
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();
@@ -457,14 +482,8 @@
}
}
}
- 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++;
@@ -505,6 +524,9 @@
send_line();
line_mutex.unlock();
}
+ run_song = true;
+ sensors.start();
+ song_thread.signal_set(0x3);
} else {
if(rev) {
line_mutex.lock();
@@ -512,27 +534,57 @@
send_line();
line_mutex.unlock();
if(vel)set_speed(n_vel);
- else set_speed(25); //Default rotation speed
- //distance_thread.start(distance);
+ else set_speed(10); //Default rotation speed
+ run_dist = true;
sensors.start();
- running_thread = &distance_thread;
+ distance_thread.signal_set(0x2);
}
if(vel) {
- set_speed(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();
+ if(!rev) {
+ set_speed(n_vel);
+ sensors.start();
+ }
}
}
+ output_main = true;
}
- new_command = true;
}
-
-
+void song_func ()
+{
+ const double freq[21] = {440, 466.16, 415.30, //A4, A_sharp4, A_flat4
+ 493.88, 523.25, 466.16, //B4
+ 261.63, 277.18, 246.94, //C4
+ 293.66, 311.13, 277.18, //D4
+ 329.63, 349.23, 311.13, //E4
+ 349.23, 369.99, 329.63, //F4
+ 392.00, 415.30, 369.99
+ }; //G4
+ while(true) {
+ Thread::signal_wait(0x3);
+ set_speed(50);
+ line_mutex.lock();
+ sprintf(tx_line, "Speeding up to play tune(5s)\r\n");
+ send_line();
+ line_mutex.unlock();
+ Thread::wait(5000);
+ for (int i=0; i<notes_length && run_song; i++) {
+ L1L.period_us(freq[notes[i]]);
+ L2L.period_us(freq[notes[i]]);
+ L3L.period_us(freq[notes[i]]);
+ line_mutex.lock();
+ sprintf(tx_line, "Playing new note: %d duration %d \r\n", notes[i], durations[i]);
+ send_line();
+ line_mutex.unlock();
+ Thread::wait(1000*durations[i]);
+ }
+ sensors.stop();
+ }
+}
// Copy tx line buffer to large tx buffer for tx interrupt routine
void send_line()
@@ -599,13 +651,13 @@
{
char c;
while ((pc.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
+ output_main = false;
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;
@@ -626,42 +678,46 @@
{
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;
+ Thread::signal_wait(0x2);
+ test = false;
+ SetDist.setSetPoint(target_distance);
+ if(target_distance<0) {
+ target_distance = target_distance*-1.0;
+ neg = -1;
}
- /*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
+ while(run_dist) {
+ 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);
+ }
}
}
}
@@ -678,8 +734,7 @@
// Setup a serial interrupt function to transmit data
pc.attach(&Tx_interrupt, Serial::TxIrq);
- int8_t orState = 0; //Rotot offset at motor state 0
- sensors.start();
+ //sensors.start();
line_mutex.lock();
sprintf(tx_line, "Hello\r\n");
send_line();
@@ -687,28 +742,13 @@
//Run the motor synchronisation
- //orState = motorHome();
+ ofstate = sensors.motor_home();
line_mutex.lock();
- sprintf(tx_line, "Rotor origin: %x\r\n",orState);
+ sprintf(tx_line, "Rotor origin: %i\r\n",ofstate);
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 count2 =0;//Count for reporting at set invertals
- int countold =turns;
- target_distance = 100;
-
- double target_rotation = 20;// The set speed in Hz
-
L1L.period_us(10);
L2L.period_us(10);
@@ -717,45 +757,39 @@
feedback_setup();
- //set_commands.start(set_commands_func);
+ count = 0;
+ turns = 0;
+
+ set_commands.start(set_commands_func);
pid_thread.start(pid_thread_func);
distance_thread.start(distance);
-
+ song_thread.start(song_func);
+ /*
+ line_mutex.lock();
+ sprintf(tx_line, "Start count %d\r\n",count);
+ send_line();
+ */
line_mutex.lock();
- sprintf(tx_line, "Start count %d\r\n",count);
+ sprintf(tx_line, "Waiting for new command\r\n");
send_line();
line_mutex.unlock();
- set_speed(target_rotation);
+ Timer t;
+ int countold = turns;
+ t.start();
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 = (turns-countold)/t.read();//calculating rotation from I1
- //pid_rotation.unlock();
-
countold = turns;
t.reset();
-
- if(new_command) {
- target_rotation = n_vel;
- target_distance = n_rev;
- count = 0;
- new_command = false;
- }
- //set_speed(target_rotation);
-
-
- if (count2>0) { //report every so often as not to slow down loop
+ if (output_main && 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 , pitch , turns, a);
+ sprintf(tx_line, "Rotation in Hz: %f %f %d %f\r\n",rotation_time , pwm_duty , turns, a);
send_line();
line_mutex.unlock();
count2 = 0;
