Sven Kugathasan / Mbed OS 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.

Dependencies:   PID

main.cpp@22:997c013e0f13, 2017-03-26 (annotated)

Committer:
af2213
Date:
Sun Mar 26 18:58:16 2017 +0000
Revision:
22:997c013e0f13
Parent:
21:828582e4d4ef 
Final debug version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
svenkugi 0:b6deec3905f4 1 /*_________________________________LIBRARIES__________________________________*/
svenkugi 0:b6deec3905f4 2
svenkugi 0:b6deec3905f4 3 #include "mbed.h"
svenkugi 0:b6deec3905f4 4 #include "rtos.h"
svenkugi 0:b6deec3905f4 5 #include "PID.h"
svenkugi 10:e974ee1ea1f0 6
svenkugi 10:e974ee1ea1f0 7 #include "ctype.h"
svenkugi 10:e974ee1ea1f0 8 #include <string>
mo713 6:8d18cdcabc3a 9 #include "stdlib.h"
svenkugi 10:e974ee1ea1f0 10 #include "math.h"
af2213 18:19d35daed140 11 #include <limits>
af2213 21:828582e4d4ef 12 //#include <vector>
svenkugi 0:b6deec3905f4 13
svenkugi 0:b6deec3905f4 14 /*_________________________________PIN SETUP__________________________________*/
svenkugi 0:b6deec3905f4 15
svenkugi 0:b6deec3905f4 16 //PhotoInterrupter Input Pins
svenkugi 0:b6deec3905f4 17 #define I1pin D2
svenkugi 0:b6deec3905f4 18 #define I2pin D11
svenkugi 0:b6deec3905f4 19 #define I3pin D12
svenkugi 0:b6deec3905f4 20
svenkugi 0:b6deec3905f4 21 //Incremental Encoder Input Pins
svenkugi 0:b6deec3905f4 22 #define CHA D7
svenkugi 0:b6deec3905f4 23 #define CHB D8
svenkugi 0:b6deec3905f4 24
svenkugi 0:b6deec3905f4 25 //Motor Drive output pins //Mask in output byte
svenkugi 0:b6deec3905f4 26 #define L1Lpin D4 //0x01
svenkugi 0:b6deec3905f4 27 #define L1Hpin D5 //0x02
svenkugi 0:b6deec3905f4 28 #define L2Lpin D3 //0x04
svenkugi 0:b6deec3905f4 29 #define L2Hpin D6 //0x08
svenkugi 0:b6deec3905f4 30 #define L3Lpin D9 //0x10
svenkugi 0:b6deec3905f4 31 #define L3Hpin D10 //0x20
svenkugi 0:b6deec3905f4 32
af2213 18:19d35daed140 33
svenkugi 0:b6deec3905f4 34 //Photointerrupter Inputs as Interrupts
svenkugi 0:b6deec3905f4 35 InterruptIn InterruptI1(D2);
svenkugi 0:b6deec3905f4 36 InterruptIn InterruptI2(D11);
svenkugi 0:b6deec3905f4 37 InterruptIn InterruptI3(D12);
svenkugi 0:b6deec3905f4 38
svenkugi 0:b6deec3905f4 39 //Incremental Encoder Inputs as Interrupts
svenkugi 0:b6deec3905f4 40 InterruptIn InterruptCHA(D7);
svenkugi 0:b6deec3905f4 41 DigitalIn InterruptCHB(D8);
svenkugi 0:b6deec3905f4 42
svenkugi 0:b6deec3905f4 43 //Motor Drive Outputs in PWM
svenkugi 0:b6deec3905f4 44 PwmOut L1L(L1Lpin);
svenkugi 0:b6deec3905f4 45 PwmOut L1H(L1Hpin);
svenkugi 0:b6deec3905f4 46 PwmOut L2L(L2Lpin);
svenkugi 0:b6deec3905f4 47 PwmOut L2H(L2Hpin);
svenkugi 0:b6deec3905f4 48 PwmOut L3L(L3Lpin);
svenkugi 0:b6deec3905f4 49 PwmOut L3H(L3Hpin);
svenkugi 0:b6deec3905f4 50
af2213 21:828582e4d4ef 51 PwmOut testOut(A5);
af2213 21:828582e4d4ef 52
svenkugi 0:b6deec3905f4 53 //Status LED
svenkugi 10:e974ee1ea1f0 54 //DigitalOut led1(LED1);
svenkugi 10:e974ee1ea1f0 55 DigitalOut led2(LED2);
svenkugi 10:e974ee1ea1f0 56 DigitalOut led3(LED3);
svenkugi 10:e974ee1ea1f0 57
svenkugi 15:b0f63ea39943 58 DigitalOut TIME(D13); //Toggle Digital Pin to measure Interrupt Times
svenkugi 0:b6deec3905f4 59
svenkugi 0:b6deec3905f4 60 //Initialise the serial port
svenkugi 0:b6deec3905f4 61 Serial pc(SERIAL_TX, SERIAL_RX);
svenkugi 0:b6deec3905f4 62
svenkugi 0:b6deec3905f4 63 //Timer
svenkugi 0:b6deec3905f4 64 Timer rps; // Measures Time for complete revolution
svenkugi 0:b6deec3905f4 65 Timer partial_rps; // Measures Time for partial revolutions
svenkugi 0:b6deec3905f4 66 Timer tmp; // Profiler Timer
svenkugi 0:b6deec3905f4 67
svenkugi 0:b6deec3905f4 68 //PID Controller
af2213 18:19d35daed140 69 PID velocity_pid(0.75, 0.025, 0.2, 0.1); // (P, I, D, WAIT)
af2213 19:d79692cef6c7 70 PID dist_pid(2, 0.0, 0.01, 0.1); // (P, I, D, WAIT)
svenkugi 10:e974ee1ea1f0 71
svenkugi 10:e974ee1ea1f0 72 //Initialize Threads
af2213 20:d796667e0c4d 73 //Thread pid_thread(osPriorityNormal, 512, NULL);
svenkugi 15:b0f63ea39943 74 Thread melody_thread(osPriorityNormal, 512, NULL);
svenkugi 0:b6deec3905f4 75
af2213 20:d796667e0c4d 76 typedef struct{
af2213 20:d796667e0c4d 77 char note;
af2213 20:d796667e0c4d 78 uint8_t sharp;
af2213 21:828582e4d4ef 79 uint8_t oct;
af2213 20:d796667e0c4d 80 float dur;
af2213 20:d796667e0c4d 81 }note_t;
af2213 20:d796667e0c4d 82
af2213 21:828582e4d4ef 83
af2213 20:d796667e0c4d 84
svenkugi 0:b6deec3905f4 85 /*________________________Motor Drive States__________________________________*/
svenkugi 0:b6deec3905f4 86
svenkugi 0:b6deec3905f4 87 //Mapping from sequential drive states to motor phase outputs
svenkugi 0:b6deec3905f4 88 /*
svenkugi 0:b6deec3905f4 89 State L1 L2 L3
svenkugi 0:b6deec3905f4 90 0 H - L
svenkugi 0:b6deec3905f4 91 1 - H L
svenkugi 0:b6deec3905f4 92 2 L H -
svenkugi 0:b6deec3905f4 93 3 L - H
svenkugi 0:b6deec3905f4 94 4 - L H
svenkugi 0:b6deec3905f4 95 5 H L -
svenkugi 0:b6deec3905f4 96 6 - - -
svenkugi 0:b6deec3905f4 97 7 - - -
svenkugi 0:b6deec3905f4 98 */
svenkugi 0:b6deec3905f4 99
svenkugi 0:b6deec3905f4 100 //Drive state to output table
svenkugi 0:b6deec3905f4 101 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
svenkugi 0:b6deec3905f4 102
svenkugi 0:b6deec3905f4 103 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
svenkugi 0:b6deec3905f4 104 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
svenkugi 0:b6deec3905f4 105
svenkugi 0:b6deec3905f4 106 /*____________________Global Variable Initialization__________________________*/
svenkugi 0:b6deec3905f4 107
svenkugi 10:e974ee1ea1f0 108 //Rotor Direction Default
svenkugi 10:e974ee1ea1f0 109 const int8_t lead = -2; //Phase lead to make motor spin: 2 for forwards, -2 for backwards
svenkugi 10:e974ee1ea1f0 110 int8_t direction = 1; //+1: Backwards rotation; -1 for Forwards Rotation
svenkugi 0:b6deec3905f4 111
svenkugi 10:e974ee1ea1f0 112 //Optical Disk States
svenkugi 10:e974ee1ea1f0 113 uint8_t orState=0; //Offset of Motor Field and Optical Disk
svenkugi 10:e974ee1ea1f0 114 uint8_t intState=0; //Current Optical Disk state
svenkugi 10:e974ee1ea1f0 115 const uint8_t num_states = 6; //Number of states in one rotation
svenkugi 0:b6deec3905f4 116
svenkugi 0:b6deec3905f4 117 uint32_t count = 0; //Counts number of states traversed
svenkugi 0:b6deec3905f4 118 int8_t completed = 0; //Checks if rotation completed
svenkugi 0:b6deec3905f4 119 int8_t driveto = 0; //Holds value of new motor drive state
svenkugi 0:b6deec3905f4 120
svenkugi 10:e974ee1ea1f0 121 //Angular Velocity Variables
af2213 21:828582e4d4ef 122 float PWM_freq = 0.00001f; //100kHz (> Motor LP cut-off frequency = 10Hz)
svenkugi 10:e974ee1ea1f0 123
svenkugi 10:e974ee1ea1f0 124 float dutyout = 1.0f; //Initialized at 50% duty cycle
svenkugi 10:e974ee1ea1f0 125 float dutyout_max = 1.0f; //Maximum Duty Cycle will enable maximum speed
svenkugi 10:e974ee1ea1f0 126
svenkugi 10:e974ee1ea1f0 127 float angular_vel = 0.0f; //Revolution per second (Measured over 360)
svenkugi 10:e974ee1ea1f0 128 float partial_vel = 0.0f; //Revolution per second (Measured over 360/117)
svenkugi 10:e974ee1ea1f0 129
svenkugi 10:e974ee1ea1f0 130 float vel_target = 0.0f; //Target Speed
svenkugi 10:e974ee1ea1f0 131 float vel_max = 100; //Maximum Speed at 3.0V achievable is ~60 rps
svenkugi 10:e974ee1ea1f0 132
svenkugi 10:e974ee1ea1f0 133 //Position Variables
svenkugi 10:e974ee1ea1f0 134 uint32_t revstates_count = 0; //Global Variable to pass into interrupt
svenkugi 10:e974ee1ea1f0 135 uint8_t pulse_count = 0; //Max.Pulse count = 117
svenkugi 10:e974ee1ea1f0 136
svenkugi 10:e974ee1ea1f0 137 float total_rev = 0.0f;
svenkugi 10:e974ee1ea1f0 138 float partial_rev = 0.0f;
svenkugi 10:e974ee1ea1f0 139
svenkugi 10:e974ee1ea1f0 140 float rev_target = 0.0f; //Target Rotations
af2213 21:828582e4d4ef 141 float revstates_max = 0;
af2213 17:209ac0b10ba1 142
svenkugi 0:b6deec3905f4 143 //Debug Variables
svenkugi 0:b6deec3905f4 144 bool flag = false;
svenkugi 10:e974ee1ea1f0 145 float test_time = 0.0f;
svenkugi 10:e974ee1ea1f0 146 int8_t test = 0;
af2213 20:d796667e0c4d 147 int songLen = 0;
svenkugi 0:b6deec3905f4 148
af2213 18:19d35daed140 149 // Timer interrupt
af2213 18:19d35daed140 150 Ticker calcPID_timer;
svenkugi 16:d426b65b4ace 151
svenkugi 0:b6deec3905f4 152 /*_____Basic Functions (Motor Drive, Synchronization, Reading Rotor State)____*/
svenkugi 0:b6deec3905f4 153
svenkugi 0:b6deec3905f4 154 //Set a given drive state
svenkugi 0:b6deec3905f4 155 void motorOut(int8_t driveState){
svenkugi 0:b6deec3905f4 156
svenkugi 0:b6deec3905f4 157 //Lookup the output byte from the drive state.
svenkugi 0:b6deec3905f4 158 int8_t driveOut = driveTable[driveState & 0x07];
svenkugi 0:b6deec3905f4 159
svenkugi 0:b6deec3905f4 160 //Turn off first (PWM)
svenkugi 0:b6deec3905f4 161 if (~driveOut & 0x01) L1L = 0;
svenkugi 0:b6deec3905f4 162 if (~driveOut & 0x02) L1H.write(dutyout); L1H.period(PWM_freq);
svenkugi 0:b6deec3905f4 163 if (~driveOut & 0x04) L2L = 0;
svenkugi 0:b6deec3905f4 164 if (~driveOut & 0x08) L2H.write(dutyout); L2H.period(PWM_freq);
svenkugi 0:b6deec3905f4 165 if (~driveOut & 0x10) L3L = 0;
svenkugi 0:b6deec3905f4 166 if (~driveOut & 0x20) L3H.write(dutyout); L3H.period(PWM_freq);
svenkugi 0:b6deec3905f4 167
svenkugi 0:b6deec3905f4 168 //Then turn on (PWM)
svenkugi 0:b6deec3905f4 169 if (driveOut & 0x01) L1L.write(dutyout); L1L.period(PWM_freq);
svenkugi 0:b6deec3905f4 170 if (driveOut & 0x02) L1H = 0;
svenkugi 0:b6deec3905f4 171 if (driveOut & 0x04) L2L.write(dutyout); L2L.period(PWM_freq);
svenkugi 0:b6deec3905f4 172 if (driveOut & 0x08) L2H = 0;
svenkugi 0:b6deec3905f4 173 if (driveOut & 0x10) L3L.write(dutyout); L3L.period(PWM_freq);
svenkugi 0:b6deec3905f4 174 if (driveOut & 0x20) L3H = 0;
svenkugi 3:7ee013b0976e 175
af2213 21:828582e4d4ef 176 testOut.write(0.5f);
af2213 21:828582e4d4ef 177 testOut.period(PWM_freq);
svenkugi 0:b6deec3905f4 178 }
af2213 18:19d35daed140 179
svenkugi 0:b6deec3905f4 180 //Convert photointerrupter inputs to a rotor state
svenkugi 0:b6deec3905f4 181 inline int8_t readRotorState(){
svenkugi 10:e974ee1ea1f0 182 return stateMap[InterruptI1.read() + 2*InterruptI2.read() + 4*InterruptI3.read()];
svenkugi 0:b6deec3905f4 183 }
svenkugi 0:b6deec3905f4 184
af2213 21:828582e4d4ef 185 const float octaveMap[] = {27.50, 30.87, 16.35, 18.35, 20.60, 21.83, 24.50};
af2213 21:828582e4d4ef 186
af2213 21:828582e4d4ef 187 float returnNote(const note_t &n)
af2213 20:d796667e0c4d 188 {
af2213 21:828582e4d4ef 189 unsigned octMult = 1;
af2213 22:997c013e0f13 190 octMult = octMult << n.oct;
af2213 21:828582e4d4ef 191 return 1.0f/( octMult*octaveMap[n.note - 'A'] );
af2213 20:d796667e0c4d 192 }
af2213 20:d796667e0c4d 193
af2213 20:d796667e0c4d 194 void playMelody(note_t* song){
af2213 20:d796667e0c4d 195 for (int i=0; i<songLen; ++i){
af2213 21:828582e4d4ef 196 //pc.printf("Playing note %c with duration %f and period %f\r\n", song[i].note, song[i].dur, returnNote(song[i]));
af2213 20:d796667e0c4d 197 PWM_freq = returnNote(song[i]);
af2213 20:d796667e0c4d 198 wait(song[i].dur);
af2213 20:d796667e0c4d 199 }
af2213 21:828582e4d4ef 200 PWM_freq = 0.00001f;
af2213 20:d796667e0c4d 201 delete[] song;
af2213 20:d796667e0c4d 202 }
af2213 20:d796667e0c4d 203
af2213 18:19d35daed140 204 void calculatePID(){
af2213 18:19d35daed140 205 //Calculate new PID Control Point
af2213 19:d79692cef6c7 206 if((total_rev/rev_target) > 0.75f){
af2213 18:19d35daed140 207 dist_pid.setProcessValue(total_rev);
af2213 18:19d35daed140 208 dutyout = dist_pid.compute();
af2213 18:19d35daed140 209 }
af2213 18:19d35daed140 210 else{
af2213 18:19d35daed140 211 velocity_pid.setProcessValue(partial_vel);
af2213 18:19d35daed140 212 dutyout = velocity_pid.compute();
af2213 18:19d35daed140 213 }
af2213 18:19d35daed140 214 }
af2213 18:19d35daed140 215
svenkugi 0:b6deec3905f4 216 //Basic synchronisation routine
svenkugi 0:b6deec3905f4 217 int8_t motorHome() {
svenkugi 10:e974ee1ea1f0 218 //Put the motor in drive state X (e.g. 5) to avoid initial jitter
svenkugi 10:e974ee1ea1f0 219 //Set to maximum speed to get maximum momentum
svenkugi 0:b6deec3905f4 220 dutyout = 1.0f;
svenkugi 0:b6deec3905f4 221 motorOut(5);
svenkugi 0:b6deec3905f4 222 wait(1.0);
svenkugi 0:b6deec3905f4 223
svenkugi 0:b6deec3905f4 224 //Put the motor in drive state 0 and wait for it to stabilise
svenkugi 0:b6deec3905f4 225 motorOut(0);
svenkugi 0:b6deec3905f4 226 wait(1.0);
svenkugi 0:b6deec3905f4 227
svenkugi 0:b6deec3905f4 228 //Get the rotor state
svenkugi 0:b6deec3905f4 229 return readRotorState();
svenkugi 0:b6deec3905f4 230 }
svenkugi 0:b6deec3905f4 231
svenkugi 0:b6deec3905f4 232 /*________________Advanced Functions (Speed and Position Control)_____________*/
svenkugi 0:b6deec3905f4 233
svenkugi 10:e974ee1ea1f0 234 // Function involves PID
af2213 18:19d35daed140 235 void position_control(float rev, float vel){
svenkugi 0:b6deec3905f4 236
af2213 18:19d35daed140 237 completed = 0;
af2213 18:19d35daed140 238 dutyout = 1.0f;
af2213 18:19d35daed140 239 count = 0;
af2213 18:19d35daed140 240
af2213 18:19d35daed140 241 rev_target = rev;
af2213 18:19d35daed140 242 vel_target = vel;
af2213 21:828582e4d4ef 243 //pc.printf("rev %f\r\n", rev);
af2213 21:828582e4d4ef 244 //pc.printf("vel %f\r\n", vel);
svenkugi 0:b6deec3905f4 245
svenkugi 10:e974ee1ea1f0 246 //Reverses motor direction if forwards rotation requested
af2213 18:19d35daed140 247 if (rev_target < 0.0f || vel_target < 0.0f){
af2213 18:19d35daed140 248 direction = -1;
af2213 18:19d35daed140 249 if (rev_target < 0.0f)
af2213 18:19d35daed140 250 rev_target = rev_target * -1;
af2213 18:19d35daed140 251 else
af2213 18:19d35daed140 252 vel_target = vel_target * -1;
svenkugi 0:b6deec3905f4 253 }
af2213 18:19d35daed140 254 else{
af2213 18:19d35daed140 255 direction = 1;
svenkugi 3:7ee013b0976e 256 }
svenkugi 3:7ee013b0976e 257
af2213 21:828582e4d4ef 258 //pc.printf("vel_target %f\r\n", vel_target);
af2213 21:828582e4d4ef 259 //pc.printf("dir %d\r\n", direction);
af2213 18:19d35daed140 260
af2213 21:828582e4d4ef 261 pc.printf("Waiting for stabilize...\r\n");
af2213 18:19d35daed140 262 dutyout = 0.0f;
af2213 19:d79692cef6c7 263 velocity_pid.reset();
af2213 19:d79692cef6c7 264 dist_pid.reset();
af2213 18:19d35daed140 265 wait(3);
af2213 18:19d35daed140 266 motorHome();
af2213 21:828582e4d4ef 267 pc.printf("Restarting...\r\n");
af2213 18:19d35daed140 268
af2213 18:19d35daed140 269 velocity_pid.reset();
af2213 19:d79692cef6c7 270 //velocity_pid.setInputLimits(0.0, 50);
svenkugi 0:b6deec3905f4 271 velocity_pid.setOutputLimits(0.0, 1.0);
svenkugi 0:b6deec3905f4 272 velocity_pid.setMode(1);
svenkugi 0:b6deec3905f4 273 velocity_pid.setSetPoint(vel_target);
svenkugi 0:b6deec3905f4 274
af2213 18:19d35daed140 275 dist_pid.reset();
af2213 19:d79692cef6c7 276 //dist_pid.setInputLimits(0.0, rev_target);
svenkugi 10:e974ee1ea1f0 277 dist_pid.setOutputLimits(0.0, 1.0);
svenkugi 0:b6deec3905f4 278 dist_pid.setMode(1);
svenkugi 0:b6deec3905f4 279 dist_pid.setSetPoint(rev_target);
af2213 18:19d35daed140 280
af2213 18:19d35daed140 281 dutyout = 0.3f;
svenkugi 0:b6deec3905f4 282 intState = readRotorState();
svenkugi 4:5eb8ac894d0f 283 driveto = (intState-orState+(direction*lead)+6)%6;
svenkugi 10:e974ee1ea1f0 284 motorOut(driveto);
svenkugi 10:e974ee1ea1f0 285
af2213 19:d79692cef6c7 286 calcPID_timer.attach(&calculatePID, 0.05);
svenkugi 0:b6deec3905f4 287 }
svenkugi 0:b6deec3905f4 288
svenkugi 0:b6deec3905f4 289 void changestate_isr(){
af2213 22:997c013e0f13 290 //TIME = 1;
svenkugi 10:e974ee1ea1f0 291 //led2 = !led2;
svenkugi 0:b6deec3905f4 292 // Profiling: Test time duration of ISR
svenkugi 0:b6deec3905f4 293 /*if(test == 0){
svenkugi 0:b6deec3905f4 294 tmp.start();
svenkugi 0:b6deec3905f4 295 test = 1;
svenkugi 0:b6deec3905f4 296 }
svenkugi 0:b6deec3905f4 297
svenkugi 0:b6deec3905f4 298 else{
svenkugi 0:b6deec3905f4 299 tmp.stop();
svenkugi 0:b6deec3905f4 300 test_time = tmp.read();
svenkugi 0:b6deec3905f4 301 tmp.reset();
svenkugi 0:b6deec3905f4 302 test = 0;
svenkugi 0:b6deec3905f4 303 }*/
svenkugi 0:b6deec3905f4 304
svenkugi 0:b6deec3905f4 305 // Measure time for 360 Rotation
svenkugi 0:b6deec3905f4 306 if(driveto == 0x04){ //Next time drivestate=4, 360 degrees revolution
svenkugi 10:e974ee1ea1f0 307 pulse_count = 0;
svenkugi 10:e974ee1ea1f0 308 /*if(flag){
svenkugi 0:b6deec3905f4 309 rps.stop();
svenkugi 0:b6deec3905f4 310 angular_vel = 1/(rps.read());
svenkugi 0:b6deec3905f4 311 rps.reset();
svenkugi 0:b6deec3905f4 312 flag = 0;
svenkugi 10:e974ee1ea1f0 313 }*/
svenkugi 0:b6deec3905f4 314 }
svenkugi 0:b6deec3905f4 315
svenkugi 10:e974ee1ea1f0 316 /*if(driveto == 0x04){ //First time drivestate=4, Timer started at 0 degrees
svenkugi 10:e974ee1ea1f0 317 pulse_count = 0; //Synchronize Quadrature Encoder with PhotoInterrupter
svenkugi 0:b6deec3905f4 318 rps.start();
svenkugi 0:b6deec3905f4 319 flag = 1;
svenkugi 10:e974ee1ea1f0 320 }*/
svenkugi 0:b6deec3905f4 321
svenkugi 0:b6deec3905f4 322 // Measure number of revolutions
svenkugi 0:b6deec3905f4 323 count++;
svenkugi 0:b6deec3905f4 324
af2213 18:19d35daed140 325 if (rev_target == std::numeric_limits<float>::max()){
af2213 18:19d35daed140 326 total_rev = 0.0f;
af2213 18:19d35daed140 327 }
af2213 18:19d35daed140 328
af2213 17:209ac0b10ba1 329 //Turn-off when target reached (if target is 0, never end)
af2213 18:19d35daed140 330 if(completed || total_rev >= rev_target){
svenkugi 0:b6deec3905f4 331 completed = 1;
af2213 18:19d35daed140 332 total_rev = 0;
af2213 18:19d35daed140 333 count = 0;
svenkugi 0:b6deec3905f4 334 dutyout = 0;
svenkugi 0:b6deec3905f4 335 motorOut(0);
svenkugi 10:e974ee1ea1f0 336 led3 = 0;
af2213 18:19d35daed140 337 partial_rev = 0;
af2213 18:19d35daed140 338 calcPID_timer.detach();
svenkugi 0:b6deec3905f4 339 }
svenkugi 0:b6deec3905f4 340 else{
svenkugi 0:b6deec3905f4 341 intState = readRotorState();
svenkugi 10:e974ee1ea1f0 342 driveto = (intState-orState+(direction*lead)+6)%6;
svenkugi 0:b6deec3905f4 343 motorOut(driveto);
svenkugi 0:b6deec3905f4 344 }
af2213 22:997c013e0f13 345 //TIME = 0;
svenkugi 0:b6deec3905f4 346 }
svenkugi 0:b6deec3905f4 347
af2213 22:997c013e0f13 348 void pid_isr(){
af2213 22:997c013e0f13 349 //TIME = 1;
svenkugi 0:b6deec3905f4 350 //117 Pulses per revolution
svenkugi 0:b6deec3905f4 351 pulse_count++;
svenkugi 0:b6deec3905f4 352
svenkugi 0:b6deec3905f4 353 //Measure Time to do 3 degrees of rotation
svenkugi 0:b6deec3905f4 354 if(test == 0){
svenkugi 0:b6deec3905f4 355 partial_rps.start();
svenkugi 0:b6deec3905f4 356 test = 1;
svenkugi 0:b6deec3905f4 357 }
svenkugi 0:b6deec3905f4 358 else{
svenkugi 0:b6deec3905f4 359 partial_rps.stop();
svenkugi 10:e974ee1ea1f0 360 partial_vel = 1/((117.0f * partial_rps.read()));
svenkugi 0:b6deec3905f4 361 partial_rps.reset();
svenkugi 0:b6deec3905f4 362 test = 0;
svenkugi 0:b6deec3905f4 363 }
svenkugi 0:b6deec3905f4 364
svenkugi 0:b6deec3905f4 365 //Partial Revolution Count
svenkugi 0:b6deec3905f4 366 partial_rev = pulse_count/117.0f;
svenkugi 0:b6deec3905f4 367
svenkugi 0:b6deec3905f4 368 //Total Revolution Count
svenkugi 0:b6deec3905f4 369 total_rev = (count/6.0f) + partial_rev;
af2213 22:997c013e0f13 370 //TIME = 0;
svenkugi 0:b6deec3905f4 371 }
svenkugi 0:b6deec3905f4 372
svenkugi 10:e974ee1ea1f0 373 /*__________________________Main Function_____________________________________*/
svenkugi 10:e974ee1ea1f0 374
svenkugi 15:b0f63ea39943 375 void serial_com(){
svenkugi 10:e974ee1ea1f0 376
af2213 12:943207547cb1 377 pc.baud(9600);
af2213 12:943207547cb1 378 float r=0;
af2213 12:943207547cb1 379 float v=0; //velocity
af2213 12:943207547cb1 380 bool r_val=true;
af2213 12:943207547cb1 381 bool v_val=true;
af2213 18:19d35daed140 382 int16_t t_loc=0;
af2213 18:19d35daed140 383 int16_t r_loc=0;
af2213 18:19d35daed140 384 int16_t v_loc=0;
af2213 18:19d35daed140 385 char buf[20];
svenkugi 10:e974ee1ea1f0 386
af2213 17:209ac0b10ba1 387 bool note_marker;
af2213 17:209ac0b10ba1 388 bool dur_marker;
af2213 17:209ac0b10ba1 389 bool accent_marker;
af2213 17:209ac0b10ba1 390 string note="";
af2213 18:19d35daed140 391 uint8_t duration=0;
af2213 22:997c013e0f13 392 Timer t;
af2213 22:997c013e0f13 393 t.start();
af2213 12:943207547cb1 394 string input;
af2213 12:943207547cb1 395 while(1){
af2213 12:943207547cb1 396 r=0;
af2213 12:943207547cb1 397 v=0;
af2213 12:943207547cb1 398 r_val=true;
af2213 12:943207547cb1 399 v_val=true;
af2213 17:209ac0b10ba1 400 note_marker=false;
af2213 17:209ac0b10ba1 401 dur_marker=false;
af2213 17:209ac0b10ba1 402 accent_marker=false;
af2213 22:997c013e0f13 403 t.stop();
af2213 22:997c013e0f13 404 pc.printf("Time taken was %f seconds. \r\n Please enter something\r\n", t.read());
af2213 12:943207547cb1 405 pc.scanf("%s",&buf);
af2213 22:997c013e0f13 406 t.reset();
af2213 22:997c013e0f13 407 t.start();
af2213 12:943207547cb1 408 input=buf;
af2213 12:943207547cb1 409 pc.printf("The input string is %s\r\n",buf);
af2213 12:943207547cb1 410
af2213 12:943207547cb1 411 t_loc=input.find('T');
af2213 12:943207547cb1 412 r_loc=input.find('R');
af2213 12:943207547cb1 413 v_loc=input.find('V');
af2213 21:828582e4d4ef 414 //pc.printf("Location of T is %d\r\n",t_loc);
af2213 21:828582e4d4ef 415 //pc.printf("Location of R is %d\r\n",r_loc);
af2213 21:828582e4d4ef 416 //pc.printf("Location of V is %d\r\n",v_loc);
svenkugi 10:e974ee1ea1f0 417
af2213 17:209ac0b10ba1 418 if(t_loc==0 && input.length()>1){ //if melody marker present and there is something after it
af2213 21:828582e4d4ef 419 //printf("Note sequence detected\r\n");
af2213 20:d796667e0c4d 420 //note_t* song = (note_t*)malloc(sizeof(note_t)*input.length());
af2213 21:828582e4d4ef 421 note_t* song = new note_t[input.length()/2]; // since each note is at least 2 characters long, max length is string/2
af2213 21:828582e4d4ef 422 songLen = 0;
af2213 17:209ac0b10ba1 423 for(int i=1;i<input.length();i++){
af2213 17:209ac0b10ba1 424
af2213 17:209ac0b10ba1 425 if(accent_marker==false && dur_marker==false && note_marker==false &&
af2213 17:209ac0b10ba1 426 (input[i]=='A' || input[i]=='B' || input[i]=='C' || input[i]=='D' || input[i]=='E' || input[i]=='F' || input[i]=='G')){
af2213 17:209ac0b10ba1 427 note_marker=true;
af2213 17:209ac0b10ba1 428 }
af2213 17:209ac0b10ba1 429 else if(note_marker==true && (input[i]=='^' || input[i]=='#')){
af2213 17:209ac0b10ba1 430 accent_marker=true;
af2213 17:209ac0b10ba1 431 }
af2213 17:209ac0b10ba1 432 else if((note_marker==true && isdigit(input[i]) && accent_marker==false) ||
af2213 17:209ac0b10ba1 433 (note_marker==true && isdigit(input[i]) && accent_marker==true)){
af2213 17:209ac0b10ba1 434 dur_marker=true;
af2213 17:209ac0b10ba1 435 }
af2213 17:209ac0b10ba1 436
af2213 17:209ac0b10ba1 437 if(note_marker==true && accent_marker==true && dur_marker == true){
af2213 17:209ac0b10ba1 438 note=input.substr(i-2,2);
af2213 21:828582e4d4ef 439 //printf("The note is %s\r\n",note.c_str());
af2213 17:209ac0b10ba1 440 duration=atof(input.substr(i,1).c_str());
af2213 21:828582e4d4ef 441 //printf("Duration is %d\r\n",duration);
af2213 17:209ac0b10ba1 442 note_marker=false;
af2213 17:209ac0b10ba1 443 dur_marker=false;
af2213 17:209ac0b10ba1 444 accent_marker=false;
af2213 21:828582e4d4ef 445 note_t newNote = {note[0], 1, 5, duration};
af2213 21:828582e4d4ef 446 song[songLen++] = newNote;
af2213 17:209ac0b10ba1 447 }
af2213 17:209ac0b10ba1 448 else if(note_marker==true && dur_marker==true && accent_marker==false){
af2213 17:209ac0b10ba1 449 note=input.substr(i-1,1);
af2213 21:828582e4d4ef 450 //printf("The note is %s\r\n",note.c_str());
af2213 17:209ac0b10ba1 451 duration=atof(input.substr(i,1).c_str());
af2213 21:828582e4d4ef 452 //printf("Duration is %d\r\n",duration);
af2213 17:209ac0b10ba1 453 note_marker=false;
af2213 17:209ac0b10ba1 454 dur_marker=false;
af2213 17:209ac0b10ba1 455 accent_marker=false;
af2213 21:828582e4d4ef 456 note_t newNote = {note[0], 1, 5, duration};
af2213 21:828582e4d4ef 457 song[songLen++] = newNote;
af2213 17:209ac0b10ba1 458 }
af2213 17:209ac0b10ba1 459 }
af2213 20:d796667e0c4d 460 melody_thread.start(callback(playMelody, song));
af2213 17:209ac0b10ba1 461 }
af2213 12:943207547cb1 462 else if(t_loc==-1){ //if no melody marker present
af2213 21:828582e4d4ef 463 //pc.printf("Note sequence NOT detected\r\n");
af2213 12:943207547cb1 464
af2213 12:943207547cb1 465 if(r_loc==0 && v_loc==-1 && input.length()>1){ //check if first letter is R
af2213 21:828582e4d4ef 466 //pc.printf("Checking for sole R input type...\r\n");
svenkugi 10:e974ee1ea1f0 467
af2213 12:943207547cb1 468 for(int j=1; j<input.length();j++){
af2213 12:943207547cb1 469 if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
af2213 12:943207547cb1 470 r_val=false;
af2213 12:943207547cb1 471 }
svenkugi 10:e974ee1ea1f0 472 }
af2213 12:943207547cb1 473
af2213 12:943207547cb1 474 if(r_val==true){
af2213 12:943207547cb1 475 r=atof(input.substr(1).c_str());
af2213 12:943207547cb1 476 pc.printf("Spin for %.3f number of rotations\r\n",r);
af2213 18:19d35daed140 477 position_control((float) r, vel_max);
svenkugi 10:e974ee1ea1f0 478 }
af2213 12:943207547cb1 479 else{
af2213 12:943207547cb1 480 pc.printf("Invalid input\r\n");
af2213 12:943207547cb1 481 }
svenkugi 10:e974ee1ea1f0 482 }
af2213 12:943207547cb1 483 else if(r_loc==0 && v_loc!=-1 && v_loc < input.length()-1){ //check if first letter is R and V is also present
af2213 21:828582e4d4ef 484 //pc.printf("Checking for combined R and V input type...\r\n");
svenkugi 10:e974ee1ea1f0 485
af2213 12:943207547cb1 486 for(int j=1; j<v_loc;j++){
af2213 12:943207547cb1 487 if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
af2213 12:943207547cb1 488 r_val=false;
af2213 12:943207547cb1 489 }
svenkugi 10:e974ee1ea1f0 490 }
af2213 12:943207547cb1 491 for(int j=v_loc+1; j<input.length();j++){
af2213 12:943207547cb1 492 if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
af2213 12:943207547cb1 493 v_val=false;
svenkugi 10:e974ee1ea1f0 494 }
svenkugi 10:e974ee1ea1f0 495 }
af2213 12:943207547cb1 496
af2213 12:943207547cb1 497 if(r_val==true && v_val==true){
af2213 12:943207547cb1 498 r=atof(input.substr(1,v_loc-1).c_str());
af2213 12:943207547cb1 499 v=atof(input.substr(v_loc+1).c_str());
af2213 12:943207547cb1 500 if(v<0){
af2213 12:943207547cb1 501 v=abs(v);
af2213 12:943207547cb1 502 }
af2213 18:19d35daed140 503 pc.printf("Spin for %.3f number of rotations at %.3f speed \r\n",r,v);
af2213 12:943207547cb1 504
af2213 18:19d35daed140 505 position_control((float) r, (float) v);
svenkugi 10:e974ee1ea1f0 506 }
af2213 12:943207547cb1 507 else{
af2213 12:943207547cb1 508 pc.printf("Invalid input\r\n");
svenkugi 10:e974ee1ea1f0 509 }
svenkugi 10:e974ee1ea1f0 510 }
af2213 12:943207547cb1 511 else if(v_loc==0 && input.length()>1){ //check if first letter is V
af2213 21:828582e4d4ef 512 //pc.printf("Checking for sole V input type...\r\n");
af2213 12:943207547cb1 513 for(int j=1; j<input.length();j++){
af2213 12:943207547cb1 514 if(!isdigit(input[j]) && input[j]!='-' && input[j]!='.'){
af2213 12:943207547cb1 515 v_val=false;
af2213 12:943207547cb1 516 }
af2213 12:943207547cb1 517 }
af2213 12:943207547cb1 518 if(v_val==true){
af2213 12:943207547cb1 519 v=atof(input.substr(1).c_str());
af2213 18:19d35daed140 520 pc.printf("Spin at %.3f speed\r\n",v);
af2213 18:19d35daed140 521 position_control(std::numeric_limits<float>::max(),(float)v);
af2213 12:943207547cb1 522 }
af2213 12:943207547cb1 523 else{
af2213 12:943207547cb1 524 pc.printf("Invalid input\r\n");
af2213 12:943207547cb1 525 }
svenkugi 10:e974ee1ea1f0 526 }
af2213 12:943207547cb1 527 else{
af2213 12:943207547cb1 528 pc.printf("Invalid input\r\n");
af2213 12:943207547cb1 529 }
svenkugi 10:e974ee1ea1f0 530 }
svenkugi 10:e974ee1ea1f0 531 }
svenkugi 10:e974ee1ea1f0 532 }
svenkugi 10:e974ee1ea1f0 533
svenkugi 15:b0f63ea39943 534
svenkugi 15:b0f63ea39943 535 int main(){
svenkugi 15:b0f63ea39943 536
svenkugi 15:b0f63ea39943 537 //Start of Program
svenkugi 15:b0f63ea39943 538 pc.printf("STARTING SKAFMO BRUSHLESS MOTOR PROJECT! \n\r");
svenkugi 15:b0f63ea39943 539 led3 = 0;
svenkugi 15:b0f63ea39943 540
svenkugi 15:b0f63ea39943 541 //Run the motor synchronisation: orState is subtracted from future rotor state inputs
svenkugi 15:b0f63ea39943 542 orState = motorHome();
af2213 21:828582e4d4ef 543 //pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %d\r\n", orState);
svenkugi 15:b0f63ea39943 544
svenkugi 15:b0f63ea39943 545 //Interrupts (Optical Disk State Change): Drives to next state, Measures whole revolution count, Measures angular velocity over a whole revolution
svenkugi 15:b0f63ea39943 546 InterruptI1.rise(&changestate_isr);
svenkugi 15:b0f63ea39943 547 InterruptI1.fall(&changestate_isr);
svenkugi 15:b0f63ea39943 548 InterruptI2.rise(&changestate_isr);
svenkugi 15:b0f63ea39943 549 InterruptI2.fall(&changestate_isr);
svenkugi 15:b0f63ea39943 550 InterruptI3.rise(&changestate_isr);
svenkugi 15:b0f63ea39943 551 InterruptI3.fall(&changestate_isr);
svenkugi 15:b0f63ea39943 552
svenkugi 15:b0f63ea39943 553 //Interrupts (Incremental Encoder CHA Phase)
svenkugi 15:b0f63ea39943 554 InterruptCHA.rise(&pid_isr);
svenkugi 15:b0f63ea39943 555
svenkugi 15:b0f63ea39943 556 //Initial Target Settings
af2213 17:209ac0b10ba1 557 //float rotation_set = 100.00;
af2213 17:209ac0b10ba1 558 //float velocity_set = 10.00;
af2213 17:209ac0b10ba1 559
svenkugi 15:b0f63ea39943 560 // Melody in a Thread
svenkugi 15:b0f63ea39943 561 // PID in Thread
svenkugi 15:b0f63ea39943 562
svenkugi 15:b0f63ea39943 563 //If speed not defined, use vel_max! If Rotation not defined, use revstates_max
svenkugi 15:b0f63ea39943 564 //float rotation_set = revstates_max;
svenkugi 15:b0f63ea39943 565 //float velocity_set = vel_max;
svenkugi 15:b0f63ea39943 566
svenkugi 15:b0f63ea39943 567
af2213 17:209ac0b10ba1 568 //pid_thread.join();
af2213 17:209ac0b10ba1 569 serial_com();
svenkugi 15:b0f63ea39943 570
svenkugi 15:b0f63ea39943 571 }
svenkugi 15:b0f63ea39943 572
svenkugi 15:b0f63ea39943 573
svenkugi 15:b0f63ea39943 574
svenkugi 0:b6deec3905f4 575 /*_______________________Testing and Tuning Function__________________________*/
svenkugi 0:b6deec3905f4 576
svenkugi 0:b6deec3905f4 577 /*Measures Angular Velocity using PhotoInterrupters by checking time taken to go
svenkugi 0:b6deec3905f4 578 from State 4 to State 4 in this case. Avoid sensor phasing as it measures one
svenkugi 0:b6deec3905f4 579 complete cycle */
svenkugi 0:b6deec3905f4 580
svenkugi 0:b6deec3905f4 581 void meas_velocity(){
svenkugi 0:b6deec3905f4 582
svenkugi 0:b6deec3905f4 583 intState = readRotorState();
svenkugi 10:e974ee1ea1f0 584 driveto = (intState-orState+(direction*lead)+6)%6;
svenkugi 0:b6deec3905f4 585 motorOut(driveto);
svenkugi 0:b6deec3905f4 586
svenkugi 0:b6deec3905f4 587 while (1) {
svenkugi 0:b6deec3905f4 588
svenkugi 0:b6deec3905f4 589 pc.printf("Rotations per second: %f \n\r", angular_vel);
svenkugi 0:b6deec3905f4 590
svenkugi 0:b6deec3905f4 591 }
svenkugi 0:b6deec3905f4 592 }
svenkugi 0:b6deec3905f4 593
svenkugi 10:e974ee1ea1f0 594 // Function has no PID
svenkugi 10:e974ee1ea1f0 595 void rotation_control(int8_t num_revs, int8_t sign){
svenkugi 10:e974ee1ea1f0 596
svenkugi 10:e974ee1ea1f0 597 revstates_count = num_revs*num_states;
svenkugi 10:e974ee1ea1f0 598
svenkugi 10:e974ee1ea1f0 599 intState = readRotorState();
svenkugi 10:e974ee1ea1f0 600 driveto = (intState-orState+(sign*lead)+6)%6;
svenkugi 10:e974ee1ea1f0 601 motorOut(driveto);
svenkugi 10:e974ee1ea1f0 602
svenkugi 10:e974ee1ea1f0 603 while(!completed){
svenkugi 10:e974ee1ea1f0 604
svenkugi 10:e974ee1ea1f0 605 //pc.printf("Angular velocity: %f \n", angular_vel);
svenkugi 10:e974ee1ea1f0 606 pc.printf("Partial Angular: %f \n", partial_vel);
svenkugi 10:e974ee1ea1f0 607 //pc.printf("Count: %d \r\n", (count/6));
svenkugi 10:e974ee1ea1f0 608
svenkugi 10:e974ee1ea1f0 609 }
svenkugi 10:e974ee1ea1f0 610 }
svenkugi 10:e974ee1ea1f0 611
svenkugi 0:b6deec3905f4 612 void PID_tuning(){
svenkugi 0:b6deec3905f4 613
svenkugi 0:b6deec3905f4 614 dutyout = 0.5;
svenkugi 0:b6deec3905f4 615
svenkugi 0:b6deec3905f4 616 intState = readRotorState();
svenkugi 0:b6deec3905f4 617 driveto = (intState-orState+lead+6)%6;
svenkugi 0:b6deec3905f4 618 motorOut(driveto);
svenkugi 0:b6deec3905f4 619
svenkugi 0:b6deec3905f4 620 while (1) {
svenkugi 0:b6deec3905f4 621
svenkugi 0:b6deec3905f4 622 // Testing Step Response by increasing D.C. from 0.5 to 0.7
svenkugi 0:b6deec3905f4 623 // Gradient is equal to Kc
svenkugi 0:b6deec3905f4 624
svenkugi 0:b6deec3905f4 625 if(count > 3000){
svenkugi 0:b6deec3905f4 626 dutyout = 0.7;
svenkugi 0:b6deec3905f4 627 }
svenkugi 0:b6deec3905f4 628
svenkugi 0:b6deec3905f4 629 pc.printf("Duty Cycle: %f ", dutyout);
svenkugi 0:b6deec3905f4 630 pc.printf("Rotations per second: %f ", angular_vel);
svenkugi 0:b6deec3905f4 631 pc.printf("Count: %d \n\r", count);
svenkugi 0:b6deec3905f4 632
svenkugi 0:b6deec3905f4 633 }
svenkugi 3:7ee013b0976e 634 }