Diff: main.cpp

Revision:

25:4b893287d750

Parent:

24:6d85dda245e1

Child:

26:7f88907d7c54

--- a/main.cpp	Fri Mar 13 20:31:22 2020 +0000
+++ b/main.cpp	Sat Apr 11 10:43:48 2020 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 #include "rtos.h"
+#include <string>
+
 //Photointerrupter input pins
 #define I1pin D3
 #define I2pin D6
@@ -30,13 +32,21 @@
 
 Serial pc(SERIAL_TX, SERIAL_RX);
 
+// Melody Variables
+int melody_length = 0;
+int melody_index = 0;
+int note_count = 1;
+float notes[16];
+int note_durations[16];
+
+
 // Controller variables
 Timer timer_velocity;
 uint32_t last_time_MtrCtlr;
 int i = 0;
 
 // Velocity Controller Variables
-float target_velocity = 30;
+float target_velocity = 40;
 float integral_vel = 0;
 float derivative_vel = 0;
 float last_vel_error = 0;
@@ -77,18 +87,18 @@
 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
 
-const float pwm_period = 0.25f;
 
 // SHARED GLOBAL VARIABLES //
 Semaphore pos_semaphore(0);
 float position = 0;
 bool direction = 1;
-
+float pwm_period = 0.25f;
 int lead = 2;
 
 // NON-SHARED GLOBAL VARIABLES //
 int lead_old;
 
+
 //Status LED
 DigitalOut led1(LED1);
 
@@ -284,9 +294,131 @@
         lead = -2;
     }
 }
+
+void process_melody(std::string input){
+
+    melody_length = 1;
+    
+    for(int i = 0; i < 16; i++){
+        notes[i] = 0;
+        note_durations[i] = 0;
+    }
+       
+    int note_index = 0;
+    for(int i = 0; i < input.length(); i++){
+        switch(input[i]){
+            case 'C':
+                if(input[i+1] == '#'){
+                    notes[note_index] = 0.003607764;
+                    i++;
+                }
+                else{
+                    notes[note_index] = 0.003822192;
+                }        
+            break;
+            case 'D':
+                if(input[i+1] == '^'){
+                    notes[note_index] = 0.003607764;
+                    i++;
+                }
+                else if(input[i+1] == '#'){
+                    notes[note_index] = 0.003214091;
+                    i++;
+                }
+                else{
+                    notes[note_index] = 0.003405299;
+                }        
+            break;
+            case 'E':
+                if(input[i+1] == '^'){
+                    notes[note_index] =0.003214091;
+                    i++;
+                }
+                else{
+                    notes[note_index] =0.003033704;
+                }        
+            break;
+            case 'F':
+                if(input[i+1] == '#'){
+                    notes[note_index] =0.002702776;
+                    i++;
+                }
+                else{
+                    notes[note_index] = 0.002863442;
+                }        
+            break;
+            case 'G':
+                if(input[i+1] == '^'){
+                    notes[note_index] = 0.002702776;
+                    i++;
+                }
+                else if(input[i+1] == '#'){
+                    notes[note_index] = 0.002407898;
+                    i++;
+                }
+                else{
+                    notes[note_index] =0.00255102;
+                }        
+            break;
+            case 'A':
+                if(input[i+1] == '^'){
+                    notes[note_index] =0.002407898;
+                    i++;
+                }
+                else if(input[i+1] == '#'){
+                    notes[note_index] = 0.002145186;
+                    i++;
+                }
+                else{
+                    notes[note_index] =0.002272727;
+                }        
+            break;
+            case 'B':
+                if(input[i+1] == '^'){
+                    notes[note_index] = 0.002145186;
+                    i++;
+                }
+                else{
+                    notes[note_index] = 0.002024783;
+                }        
+            break;
+        }
+        
+        
+        i++;
+        // convert -'0' to convert char to int
+        note_durations[note_index] = input[i] - '0';
+       
+        note_index++;
+        melody_length ++;
+
+    }
+    
+}
+    
+
+
+float get_period(){
+    
+    int curr_note_length = note_durations[melody_index];
+    float curr_note_period = notes[melody_index];
+    
+    if( note_count >= curr_note_length ){
+        
+        melody_index = (melody_index + 1)%melody_length; 
+        note_count = 1;
+    }
+    else{
+        note_count = note_count + 1;
+    }
+
+    return curr_note_period;
+}    
+
+
 void motorInitSequence()
 {
-    motorCtrlTicker.attach_us(&motorCtrlTick, 100000);
+    motorCtrlTicker.attach_us(&motorCtrlTick, 125000);
     last_pos_error = target_position;
     last_time_MtrCtlr = 0;
 
@@ -321,6 +453,7 @@
     timer_velocity.start();
 }
 
+
 void motorCtrlFn()
 {
     while (1)
@@ -328,6 +461,7 @@
         thread_motorCtrl.signal_wait(0x1);
 
         current_position = get_current_position();
+        //pwm_period = get_period();
         float current_velocity = get_current_velocity(current_position);
 
         float velocity_error = target_velocity - current_velocity;
@@ -338,23 +472,25 @@
 
         float velocity_out = combine_control_out(position_control_out, velocity_control_out, current_velocity);
         float motor_out = get_motor_out(position_control_out);
-
+        
         update_lead(velocity_out);
         MotorPWM.period(pwm_period);
         MotorPWM.write(motor_out);
-
-        if (i > 10)
-        {
-            pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", current_velocity, (position / 6), motor_out, velocity_out, lead);
-            i = 0;
-        }
-        i++;
+        
+       // pc.printf("Period = %f, melody_index = %d, note_count = %d\r\n", pwm_period, melody_index, note_count);
     }
 }
 
 //Main
 int main()
 {
+    process_melody("A4C8G4F#8");
+    
+    /*
+    pc.printf("A = %f, C = %f, G = %f, F# = %f\r\n", notes[0], notes[1], notes[2], notes[3]);
+    pc.printf("length A = %d, length C = %d, length G = %d, length F# = %d\r\n", note_durations[0], note_durations[1], note_durations[2], note_durations[3]);
+    */
+    
     motorInitSequence();
     thread_motorCtrl.start(motorCtrlFn);
 }