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@6:8d18cdcabc3a, 2017-03-24 (annotated)

Committer:
mo713
Date:
Fri Mar 24 02:10:11 2017 +0000
Revision:
6:8d18cdcabc3a
Parent:
4:5eb8ac894d0f
Child:
7:3524b21c3828
Child:
10:e974ee1ea1f0
regexx

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"
mo713 6:8d18cdcabc3a 6 #include "Regexp.h"
mo713 6:8d18cdcabc3a 7 #include "string.h"
mo713 6:8d18cdcabc3a 8 #include "stdlib.h"
mo713 6:8d18cdcabc3a 9 #include <string>
mo713 6:8d18cdcabc3a 10 #include <ctype.h>
svenkugi 0:b6deec3905f4 11
svenkugi 0:b6deec3905f4 12 /*_________________________________PIN SETUP__________________________________*/
svenkugi 0:b6deec3905f4 13
svenkugi 0:b6deec3905f4 14 //PhotoInterrupter Input Pins
svenkugi 0:b6deec3905f4 15 #define I1pin D2
svenkugi 0:b6deec3905f4 16 #define I2pin D11
svenkugi 0:b6deec3905f4 17 #define I3pin D12
svenkugi 0:b6deec3905f4 18
svenkugi 0:b6deec3905f4 19 //Incremental Encoder Input Pins
svenkugi 0:b6deec3905f4 20 #define CHA D7
svenkugi 0:b6deec3905f4 21 #define CHB D8
svenkugi 0:b6deec3905f4 22
svenkugi 0:b6deec3905f4 23 //Motor Drive output pins //Mask in output byte
svenkugi 0:b6deec3905f4 24 #define L1Lpin D4 //0x01
svenkugi 0:b6deec3905f4 25 #define L1Hpin D5 //0x02
svenkugi 0:b6deec3905f4 26 #define L2Lpin D3 //0x04
svenkugi 0:b6deec3905f4 27 #define L2Hpin D6 //0x08
svenkugi 0:b6deec3905f4 28 #define L3Lpin D9 //0x10
svenkugi 0:b6deec3905f4 29 #define L3Hpin D10 //0x20
svenkugi 0:b6deec3905f4 30
svenkugi 0:b6deec3905f4 31 //Photointerrupter Inputs as Interrupts
svenkugi 0:b6deec3905f4 32 InterruptIn InterruptI1(D2);
svenkugi 0:b6deec3905f4 33 InterruptIn InterruptI2(D11);
svenkugi 0:b6deec3905f4 34 InterruptIn InterruptI3(D12);
svenkugi 0:b6deec3905f4 35
svenkugi 0:b6deec3905f4 36 //Incremental Encoder Inputs as Interrupts
svenkugi 0:b6deec3905f4 37 InterruptIn InterruptCHA(D7);
svenkugi 0:b6deec3905f4 38 DigitalIn InterruptCHB(D8);
svenkugi 0:b6deec3905f4 39
svenkugi 0:b6deec3905f4 40 //Motor Drive Outputs in PWM
svenkugi 0:b6deec3905f4 41 PwmOut L1L(L1Lpin);
svenkugi 0:b6deec3905f4 42 PwmOut L1H(L1Hpin);
svenkugi 0:b6deec3905f4 43 PwmOut L2L(L2Lpin);
svenkugi 0:b6deec3905f4 44 PwmOut L2H(L2Hpin);
svenkugi 0:b6deec3905f4 45 PwmOut L3L(L3Lpin);
svenkugi 0:b6deec3905f4 46 PwmOut L3H(L3Hpin);
svenkugi 0:b6deec3905f4 47
svenkugi 0:b6deec3905f4 48 //Status LED
svenkugi 0:b6deec3905f4 49 DigitalOut led1(LED1);
svenkugi 0:b6deec3905f4 50
svenkugi 0:b6deec3905f4 51 //Initialise the serial port
svenkugi 0:b6deec3905f4 52 Serial pc(SERIAL_TX, SERIAL_RX);
svenkugi 0:b6deec3905f4 53
svenkugi 0:b6deec3905f4 54 //Initialize Threads
svenkugi 0:b6deec3905f4 55 Thread thread;
svenkugi 0:b6deec3905f4 56
svenkugi 0:b6deec3905f4 57 //Timer
svenkugi 0:b6deec3905f4 58 Timer rps; // Measures Time for complete revolution
svenkugi 0:b6deec3905f4 59 Timer partial_rps; // Measures Time for partial revolutions
svenkugi 0:b6deec3905f4 60 Timer tmp; // Profiler Timer
svenkugi 0:b6deec3905f4 61
svenkugi 0:b6deec3905f4 62 //PID Controller
svenkugi 0:b6deec3905f4 63 PID velocity_pid(0.5, 0.5, 0.1, 0.05); // (P, I, D, WAIT)
svenkugi 0:b6deec3905f4 64 PID dist_pid(2, 0.0, 0.005, 0.05); // (P, I, D, WAIT)
svenkugi 0:b6deec3905f4 65
svenkugi 0:b6deec3905f4 66 /*________________________Motor Drive States__________________________________*/
svenkugi 0:b6deec3905f4 67
svenkugi 0:b6deec3905f4 68 //Mapping from sequential drive states to motor phase outputs
svenkugi 0:b6deec3905f4 69 /*
svenkugi 0:b6deec3905f4 70 State L1 L2 L3
svenkugi 0:b6deec3905f4 71 0 H - L
svenkugi 0:b6deec3905f4 72 1 - H L
svenkugi 0:b6deec3905f4 73 2 L H -
svenkugi 0:b6deec3905f4 74 3 L - H
svenkugi 0:b6deec3905f4 75 4 - L H
svenkugi 0:b6deec3905f4 76 5 H L -
svenkugi 0:b6deec3905f4 77 6 - - -
svenkugi 0:b6deec3905f4 78 7 - - -
svenkugi 0:b6deec3905f4 79 */
svenkugi 0:b6deec3905f4 80
svenkugi 0:b6deec3905f4 81 //Drive state to output table
svenkugi 0:b6deec3905f4 82 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
svenkugi 0:b6deec3905f4 83
svenkugi 0:b6deec3905f4 84 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
svenkugi 0:b6deec3905f4 85 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
svenkugi 0:b6deec3905f4 86
svenkugi 0:b6deec3905f4 87 //Alternative if phase order of input or drive is reversed
svenkugi 0:b6deec3905f4 88 //const int8_t stateMap_Reverse[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07};
svenkugi 0:b6deec3905f4 89
svenkugi 0:b6deec3905f4 90 /*____________________Global Variable Initialization__________________________*/
svenkugi 0:b6deec3905f4 91
svenkugi 0:b6deec3905f4 92 //Phase lead to make motor spin: 2 for forwards, -2 for backwards
svenkugi 0:b6deec3905f4 93 const int8_t lead = -2;
svenkugi 0:b6deec3905f4 94 int8_t direction = 1; //Default Setting: Backwards rotation
svenkugi 0:b6deec3905f4 95
svenkugi 0:b6deec3905f4 96 //Rotor Offset
svenkugi 0:b6deec3905f4 97 uint8_t orState=0;
svenkugi 0:b6deec3905f4 98
svenkugi 0:b6deec3905f4 99 //Disk State
svenkugi 0:b6deec3905f4 100 uint8_t intState=0; //Current Optical Disk state
svenkugi 0:b6deec3905f4 101
svenkugi 0:b6deec3905f4 102 //State Variables
svenkugi 0:b6deec3905f4 103 const uint8_t num_states = 6; //Number of states in one rotation
svenkugi 0:b6deec3905f4 104 uint32_t count = 0; //Counts number of states traversed
svenkugi 0:b6deec3905f4 105 int8_t completed = 0; //Checks if rotation completed
svenkugi 0:b6deec3905f4 106 int8_t driveto = 0; //Holds value of new motor drive state
svenkugi 0:b6deec3905f4 107
svenkugi 0:b6deec3905f4 108 //Debug Variables
svenkugi 0:b6deec3905f4 109 bool flag = false;
svenkugi 0:b6deec3905f4 110 float test_time=0;
svenkugi 0:b6deec3905f4 111 uint8_t test = 0;
svenkugi 0:b6deec3905f4 112
svenkugi 0:b6deec3905f4 113 //Angular Velocity Variables
svenkugi 0:b6deec3905f4 114 float period = 0; //Time taken to complete one rotation
svenkugi 0:b6deec3905f4 115 float angular_vel = 0; //Revolution per second
svenkugi 0:b6deec3905f4 116 float test_vel = 0;
svenkugi 0:b6deec3905f4 117
svenkugi 0:b6deec3905f4 118 float PWM_freq = 0.001f; //500Hz (> Motor LP cut-off frequency = 10Hz)
svenkugi 0:b6deec3905f4 119 float dutyout = 1.0f; //Initialized at 50% duty cycle
svenkugi 0:b6deec3905f4 120 float dutyout_max = 1.0f; //Maximum Duty Cycle
svenkugi 0:b6deec3905f4 121
svenkugi 0:b6deec3905f4 122 //float velocity_set = 10.0;
svenkugi 0:b6deec3905f4 123 uint32_t revstates_count = 0; //Global Variable to pass into interrupt
svenkugi 0:b6deec3905f4 124 float rev_target = 0.0f;
svenkugi 0:b6deec3905f4 125 float vel_target = 10.0f;
svenkugi 0:b6deec3905f4 126
svenkugi 0:b6deec3905f4 127 uint8_t pulse_count = 0;
svenkugi 0:b6deec3905f4 128 float total_rev = 0.0f;
svenkugi 0:b6deec3905f4 129 float partial_rev = 0.0f;
svenkugi 0:b6deec3905f4 130 float drive_vel = 0.0f;
svenkugi 0:b6deec3905f4 131
svenkugi 0:b6deec3905f4 132 /*_____Basic Functions (Motor Drive, Synchronization, Reading Rotor State)____*/
svenkugi 0:b6deec3905f4 133
svenkugi 0:b6deec3905f4 134 //Set a given drive state
svenkugi 0:b6deec3905f4 135 void motorOut(int8_t driveState){
svenkugi 0:b6deec3905f4 136
svenkugi 0:b6deec3905f4 137 //Lookup the output byte from the drive state.
svenkugi 0:b6deec3905f4 138 int8_t driveOut = driveTable[driveState & 0x07];
svenkugi 0:b6deec3905f4 139
svenkugi 0:b6deec3905f4 140 //Turn off first (PWM)
svenkugi 0:b6deec3905f4 141 if (~driveOut & 0x01) L1L = 0;
svenkugi 0:b6deec3905f4 142 if (~driveOut & 0x02) L1H.write(dutyout); L1H.period(PWM_freq);
svenkugi 0:b6deec3905f4 143 if (~driveOut & 0x04) L2L = 0;
svenkugi 0:b6deec3905f4 144 if (~driveOut & 0x08) L2H.write(dutyout); L2H.period(PWM_freq);
svenkugi 0:b6deec3905f4 145 if (~driveOut & 0x10) L3L = 0;
svenkugi 0:b6deec3905f4 146 if (~driveOut & 0x20) L3H.write(dutyout); L3H.period(PWM_freq);
svenkugi 0:b6deec3905f4 147
svenkugi 0:b6deec3905f4 148 //Then turn on (PWM)
svenkugi 0:b6deec3905f4 149 if (driveOut & 0x01) L1L.write(dutyout); L1L.period(PWM_freq);
svenkugi 0:b6deec3905f4 150 if (driveOut & 0x02) L1H = 0;
svenkugi 0:b6deec3905f4 151 if (driveOut & 0x04) L2L.write(dutyout); L2L.period(PWM_freq);
svenkugi 0:b6deec3905f4 152 if (driveOut & 0x08) L2H = 0;
svenkugi 0:b6deec3905f4 153 if (driveOut & 0x10) L3L.write(dutyout); L3L.period(PWM_freq);
svenkugi 0:b6deec3905f4 154 if (driveOut & 0x20) L3H = 0;
svenkugi 3:7ee013b0976e 155
svenkugi 0:b6deec3905f4 156 }
svenkugi 0:b6deec3905f4 157
svenkugi 0:b6deec3905f4 158 //Convert photointerrupter inputs to a rotor state
svenkugi 0:b6deec3905f4 159 inline int8_t readRotorState(){
svenkugi 0:b6deec3905f4 160 return stateMap[InterruptI1.read() + 2*InterruptI2.read() + 4*InterruptI3.read()];
svenkugi 0:b6deec3905f4 161 }
svenkugi 0:b6deec3905f4 162
svenkugi 0:b6deec3905f4 163 //Basic synchronisation routine
svenkugi 0:b6deec3905f4 164 int8_t motorHome() {
svenkugi 0:b6deec3905f4 165 dutyout = 1.0f;
svenkugi 0:b6deec3905f4 166
svenkugi 3:7ee013b0976e 167 //Put the motor in drive state X (e.g. 5) tp avoid initial jittevbo
svenkugi 0:b6deec3905f4 168 motorOut(5);
svenkugi 0:b6deec3905f4 169 wait(1.0);
svenkugi 0:b6deec3905f4 170
svenkugi 0:b6deec3905f4 171 //Put the motor in drive state 0 and wait for it to stabilise
svenkugi 0:b6deec3905f4 172 motorOut(0);
svenkugi 0:b6deec3905f4 173 wait(1.0);
svenkugi 0:b6deec3905f4 174
svenkugi 0:b6deec3905f4 175 //Get the rotor state
svenkugi 0:b6deec3905f4 176 return readRotorState();
svenkugi 0:b6deec3905f4 177 }
mo713 6:8d18cdcabc3a 178 void serial_com()
mo713 6:8d18cdcabc3a 179 {
mo713 6:8d18cdcabc3a 180
mo713 6:8d18cdcabc3a 181 pc.baud(9600);
mo713 6:8d18cdcabc3a 182
mo713 6:8d18cdcabc3a 183 string input;
mo713 6:8d18cdcabc3a 184
mo713 6:8d18cdcabc3a 185 int position_r;
mo713 6:8d18cdcabc3a 186 int position_v;
mo713 6:8d18cdcabc3a 187
mo713 6:8d18cdcabc3a 188 float r;
mo713 6:8d18cdcabc3a 189 float v;
mo713 6:8d18cdcabc3a 190 string note;
mo713 6:8d18cdcabc3a 191 int duration;
mo713 6:8d18cdcabc3a 192 int ind1=1;
mo713 6:8d18cdcabc3a 193 int ind2=0;
mo713 6:8d18cdcabc3a 194
mo713 6:8d18cdcabc3a 195
mo713 6:8d18cdcabc3a 196 char temp [255];
mo713 6:8d18cdcabc3a 197
mo713 6:8d18cdcabc3a 198 /////////////
mo713 6:8d18cdcabc3a 199
mo713 6:8d18cdcabc3a 200
mo713 6:8d18cdcabc3a 201 while (1) {
mo713 6:8d18cdcabc3a 202
mo713 6:8d18cdcabc3a 203 pc.printf("Please enter\r\n");
mo713 6:8d18cdcabc3a 204 pc.scanf("%s",&temp);
mo713 6:8d18cdcabc3a 205 input=temp;
mo713 6:8d18cdcabc3a 206 const char *buf = input.c_str();
mo713 6:8d18cdcabc3a 207 MatchState ms;
mo713 6:8d18cdcabc3a 208 ms.Target ((char*)buf);
mo713 6:8d18cdcabc3a 209
mo713 6:8d18cdcabc3a 210 char note_start=ms.Match("^[T]");
mo713 6:8d18cdcabc3a 211
mo713 6:8d18cdcabc3a 212 char note_mask = ms.Match ("[ABCDEFG]([#^]?)[1-8]$");
mo713 6:8d18cdcabc3a 213
mo713 6:8d18cdcabc3a 214 char mv_mask = ms.Match ("^([RV]([-]?)(%d+)([.]?)(%d*))(([V]?)([-]?)(%d*)([.]?)(%d*))$");
mo713 6:8d18cdcabc3a 215
mo713 6:8d18cdcabc3a 216 if (note_mask == REGEXP_MATCHED && note_start==REGEXP_MATCHED)
mo713 6:8d18cdcabc3a 217 {
mo713 6:8d18cdcabc3a 218 pc.printf("Note sequence match\r\n");
mo713 6:8d18cdcabc3a 219 pc.printf("Note sequence is %s\r\n",input.substr(1).c_str());
mo713 6:8d18cdcabc3a 220 pc.printf("Durations are ...\r\n");
mo713 6:8d18cdcabc3a 221 for(int i=1;i<=input.length();i++)
mo713 6:8d18cdcabc3a 222 {
mo713 6:8d18cdcabc3a 223
mo713 6:8d18cdcabc3a 224 if(isdigit(input[i]))
mo713 6:8d18cdcabc3a 225 {
mo713 6:8d18cdcabc3a 226
mo713 6:8d18cdcabc3a 227 duration=(int)(input[i]-'0');
mo713 6:8d18cdcabc3a 228 pc.printf("The note is %s with duration %d\r\n",input.substr(ind1,i-ind1).c_str(),duration);
mo713 6:8d18cdcabc3a 229
mo713 6:8d18cdcabc3a 230 ind1=i+1;
mo713 6:8d18cdcabc3a 231 }
mo713 6:8d18cdcabc3a 232
mo713 6:8d18cdcabc3a 233 }
mo713 6:8d18cdcabc3a 234
mo713 6:8d18cdcabc3a 235 }
mo713 6:8d18cdcabc3a 236
mo713 6:8d18cdcabc3a 237
mo713 6:8d18cdcabc3a 238 else if (note_start == REGEXP_NOMATCH || note_mask==REGEXP_NOMATCH)
mo713 6:8d18cdcabc3a 239 {
mo713 6:8d18cdcabc3a 240 if (mv_mask == REGEXP_MATCHED)
mo713 6:8d18cdcabc3a 241 {
mo713 6:8d18cdcabc3a 242 pc.printf("Match\r\n");
mo713 6:8d18cdcabc3a 243 position_r=input.find('R');
mo713 6:8d18cdcabc3a 244 position_v=input.find('V');
mo713 6:8d18cdcabc3a 245
mo713 6:8d18cdcabc3a 246 pc.printf("Position of r is %d\r\n",position_r);
mo713 6:8d18cdcabc3a 247 pc.printf("Position of v is %d\r\n",position_v);
mo713 6:8d18cdcabc3a 248
mo713 6:8d18cdcabc3a 249
mo713 6:8d18cdcabc3a 250 if(position_v==-1)
mo713 6:8d18cdcabc3a 251 {
mo713 6:8d18cdcabc3a 252 r=atof(input.substr(position_r+1).c_str());
mo713 6:8d18cdcabc3a 253 pc.printf("Sping for %.3f rotations\r\n",r);
mo713 6:8d18cdcabc3a 254 }
mo713 6:8d18cdcabc3a 255
mo713 6:8d18cdcabc3a 256 else if(position_r==-1)
mo713 6:8d18cdcabc3a 257 {
mo713 6:8d18cdcabc3a 258 v=atof(input.substr(position_v+1).c_str());
mo713 6:8d18cdcabc3a 259 pc.printf("Spin at speed of %.3f indefinitely\r\n",v);
mo713 6:8d18cdcabc3a 260 }
mo713 6:8d18cdcabc3a 261
mo713 6:8d18cdcabc3a 262 else
mo713 6:8d18cdcabc3a 263 {
mo713 6:8d18cdcabc3a 264
mo713 6:8d18cdcabc3a 265 if(input.at(position_v+1)=='-')
mo713 6:8d18cdcabc3a 266 {
mo713 6:8d18cdcabc3a 267 r=atof(input.substr(position_r+1,position_v-1).c_str());
mo713 6:8d18cdcabc3a 268 v=atof(input.substr(position_v+2).c_str());
mo713 6:8d18cdcabc3a 269 }
mo713 6:8d18cdcabc3a 270 else
mo713 6:8d18cdcabc3a 271 {
mo713 6:8d18cdcabc3a 272 r=atof(input.substr(position_r+1,position_v-1).c_str());
mo713 6:8d18cdcabc3a 273 v=atof(input.substr(position_v+1).c_str());
mo713 6:8d18cdcabc3a 274 }
mo713 6:8d18cdcabc3a 275 pc.printf("Spin for %.3f rotations at speed of %.3f\r\n",r,v);
mo713 6:8d18cdcabc3a 276
mo713 6:8d18cdcabc3a 277 }
mo713 6:8d18cdcabc3a 278
mo713 6:8d18cdcabc3a 279
mo713 6:8d18cdcabc3a 280
mo713 6:8d18cdcabc3a 281
mo713 6:8d18cdcabc3a 282 }
mo713 6:8d18cdcabc3a 283
mo713 6:8d18cdcabc3a 284 else if(mv_mask==REGEXP_NOMATCH)
mo713 6:8d18cdcabc3a 285 pc.printf("Invalid input\r\n");
mo713 6:8d18cdcabc3a 286 }
mo713 6:8d18cdcabc3a 287 else
mo713 6:8d18cdcabc3a 288 {
mo713 6:8d18cdcabc3a 289 pc.printf("Mask error\r\n");
mo713 6:8d18cdcabc3a 290 }
mo713 6:8d18cdcabc3a 291
mo713 6:8d18cdcabc3a 292
mo713 6:8d18cdcabc3a 293
mo713 6:8d18cdcabc3a 294 }
mo713 6:8d18cdcabc3a 295
mo713 6:8d18cdcabc3a 296
mo713 6:8d18cdcabc3a 297 }
mo713 6:8d18cdcabc3a 298
svenkugi 0:b6deec3905f4 299
svenkugi 0:b6deec3905f4 300 /*________________Advanced Functions (Speed and Position Control)_____________*/
svenkugi 0:b6deec3905f4 301
svenkugi 0:b6deec3905f4 302 // Function has no PID
svenkugi 0:b6deec3905f4 303 void rotation_control(int8_t num_revs, int8_t sign){
svenkugi 0:b6deec3905f4 304
svenkugi 0:b6deec3905f4 305 revstates_count = num_revs*num_states;
svenkugi 0:b6deec3905f4 306
svenkugi 0:b6deec3905f4 307 intState = readRotorState();
svenkugi 0:b6deec3905f4 308 driveto = (intState-orState+(sign*lead)+6)%6;
svenkugi 0:b6deec3905f4 309 motorOut(driveto);
svenkugi 0:b6deec3905f4 310
svenkugi 0:b6deec3905f4 311 while(!completed){
svenkugi 0:b6deec3905f4 312
svenkugi 0:b6deec3905f4 313 //pc.printf("Angular velocity: %f \n", angular_vel);
svenkugi 0:b6deec3905f4 314 pc.printf("Partial Angular: %f \n", test_vel);
svenkugi 0:b6deec3905f4 315 //pc.printf("Count: %d \r\n", (count/6));
svenkugi 0:b6deec3905f4 316
svenkugi 0:b6deec3905f4 317 }
svenkugi 0:b6deec3905f4 318 }
svenkugi 0:b6deec3905f4 319
svenkugi 0:b6deec3905f4 320 // Funciton involves PID
svenkugi 3:7ee013b0976e 321 void position_control(float num_revs, float vel_target){
svenkugi 0:b6deec3905f4 322
svenkugi 3:7ee013b0976e 323 if(num_revs < 0){
svenkugi 3:7ee013b0976e 324 direction = -1;
svenkugi 3:7ee013b0976e 325 }
svenkugi 3:7ee013b0976e 326
svenkugi 3:7ee013b0976e 327 rev_target = abs(num_revs);
svenkugi 0:b6deec3905f4 328
svenkugi 0:b6deec3905f4 329 velocity_pid.setInputLimits(0.0, 50.0);
svenkugi 0:b6deec3905f4 330 velocity_pid.setOutputLimits(0.0, 1.0);
svenkugi 0:b6deec3905f4 331 velocity_pid.setMode(1);
svenkugi 0:b6deec3905f4 332 velocity_pid.setSetPoint(vel_target);
svenkugi 0:b6deec3905f4 333
svenkugi 0:b6deec3905f4 334 dist_pid.setInputLimits(0.0, 1000.0);
svenkugi 0:b6deec3905f4 335 dist_pid.setOutputLimits(0.2, 1.0);
svenkugi 0:b6deec3905f4 336 dist_pid.setMode(1);
svenkugi 0:b6deec3905f4 337 dist_pid.setSetPoint(rev_target);
svenkugi 0:b6deec3905f4 338
svenkugi 0:b6deec3905f4 339 intState = readRotorState();
svenkugi 4:5eb8ac894d0f 340 driveto = (intState-orState+(direction*lead)+6)%6;
svenkugi 0:b6deec3905f4 341 motorOut(driveto);
svenkugi 0:b6deec3905f4 342
svenkugi 0:b6deec3905f4 343 while(!completed){
svenkugi 0:b6deec3905f4 344
svenkugi 0:b6deec3905f4 345 //pc.printf("dutyout: %f \r\n", dutyout);
svenkugi 0:b6deec3905f4 346 pc.printf("Angular velocity: %f \r\n", angular_vel);
svenkugi 0:b6deec3905f4 347 pc.printf("Count: %d \r\n", count);
svenkugi 0:b6deec3905f4 348 //pc.printf("Total_rev: %f \r\n", total_rev);
svenkugi 0:b6deec3905f4 349 //pc.printf("\n");
svenkugi 0:b6deec3905f4 350 wait(0.05);
svenkugi 0:b6deec3905f4 351 }
svenkugi 0:b6deec3905f4 352
svenkugi 0:b6deec3905f4 353 }
svenkugi 0:b6deec3905f4 354
svenkugi 0:b6deec3905f4 355 void changestate_isr(){
svenkugi 0:b6deec3905f4 356
svenkugi 0:b6deec3905f4 357 // Profiling: Test time duration of ISR
svenkugi 0:b6deec3905f4 358 /*if(test == 0){
svenkugi 0:b6deec3905f4 359 tmp.start();
svenkugi 0:b6deec3905f4 360 test = 1;
svenkugi 0:b6deec3905f4 361 }
svenkugi 0:b6deec3905f4 362
svenkugi 0:b6deec3905f4 363 else{
svenkugi 0:b6deec3905f4 364 tmp.stop();
svenkugi 0:b6deec3905f4 365 test_time = tmp.read();
svenkugi 0:b6deec3905f4 366 tmp.reset();
svenkugi 0:b6deec3905f4 367 test = 0;
svenkugi 0:b6deec3905f4 368 }*/
svenkugi 0:b6deec3905f4 369
svenkugi 0:b6deec3905f4 370 // Measure time for 360 Rotation
svenkugi 0:b6deec3905f4 371 if(driveto == 0x04){ //Next time drivestate=4, 360 degrees revolution
svenkugi 0:b6deec3905f4 372 if(flag){
svenkugi 0:b6deec3905f4 373 rps.stop();
svenkugi 0:b6deec3905f4 374 angular_vel = 1/(rps.read());
svenkugi 0:b6deec3905f4 375 rps.reset();
svenkugi 0:b6deec3905f4 376 flag = 0;
svenkugi 0:b6deec3905f4 377 }
svenkugi 0:b6deec3905f4 378 }
svenkugi 0:b6deec3905f4 379
svenkugi 0:b6deec3905f4 380 if(driveto == 0x04){ //First time drivestate=4, Timer started at 0 degrees
svenkugi 0:b6deec3905f4 381 pulse_count = 0;
svenkugi 0:b6deec3905f4 382 rps.start();
svenkugi 0:b6deec3905f4 383 flag = 1;
svenkugi 0:b6deec3905f4 384 }
svenkugi 0:b6deec3905f4 385
svenkugi 0:b6deec3905f4 386 // Measure number of revolutions
svenkugi 0:b6deec3905f4 387 count++;
svenkugi 0:b6deec3905f4 388
svenkugi 0:b6deec3905f4 389 //Turn-off when target reached
svenkugi 0:b6deec3905f4 390 if(total_rev >= rev_target){
svenkugi 0:b6deec3905f4 391 completed = 1;
svenkugi 0:b6deec3905f4 392 dutyout = 0;
svenkugi 0:b6deec3905f4 393 motorOut(0);
svenkugi 0:b6deec3905f4 394 led1 = 0;
svenkugi 0:b6deec3905f4 395 __disable_irq();
svenkugi 0:b6deec3905f4 396 }
svenkugi 0:b6deec3905f4 397 else{
svenkugi 0:b6deec3905f4 398 intState = readRotorState();
svenkugi 0:b6deec3905f4 399 driveto = (intState-orState+direction*lead+6)%6;
svenkugi 0:b6deec3905f4 400 motorOut(driveto);
svenkugi 0:b6deec3905f4 401 }
svenkugi 0:b6deec3905f4 402
svenkugi 0:b6deec3905f4 403 }
svenkugi 0:b6deec3905f4 404
svenkugi 0:b6deec3905f4 405 void pid_isr(){
svenkugi 0:b6deec3905f4 406
svenkugi 0:b6deec3905f4 407 led1 = !led1;
svenkugi 0:b6deec3905f4 408 //tmp.start();
svenkugi 0:b6deec3905f4 409
svenkugi 0:b6deec3905f4 410 //117 Pulses per revolution
svenkugi 0:b6deec3905f4 411 pulse_count++;
svenkugi 0:b6deec3905f4 412
svenkugi 0:b6deec3905f4 413 //Measure Time to do 3 degrees of rotation
svenkugi 0:b6deec3905f4 414 if(test == 0){
svenkugi 0:b6deec3905f4 415 partial_rps.start();
svenkugi 0:b6deec3905f4 416 test = 1;
svenkugi 0:b6deec3905f4 417 }
svenkugi 0:b6deec3905f4 418 else{
svenkugi 0:b6deec3905f4 419 partial_rps.stop();
svenkugi 0:b6deec3905f4 420 test_vel = 1/((117.0f * partial_rps.read()));
svenkugi 0:b6deec3905f4 421 partial_rps.reset();
svenkugi 0:b6deec3905f4 422 test = 0;
svenkugi 0:b6deec3905f4 423 }
svenkugi 0:b6deec3905f4 424
svenkugi 0:b6deec3905f4 425 //Partial Revolution Count
svenkugi 0:b6deec3905f4 426 partial_rev = pulse_count/117.0f;
svenkugi 0:b6deec3905f4 427
svenkugi 0:b6deec3905f4 428 //Total Revolution Count
svenkugi 0:b6deec3905f4 429 total_rev = (count/6.0f) + partial_rev;
svenkugi 0:b6deec3905f4 430
svenkugi 0:b6deec3905f4 431 //Calculate new PID Control Point
svenkugi 0:b6deec3905f4 432 if((total_rev/rev_target) > 0.7f){
svenkugi 0:b6deec3905f4 433 dist_pid.setProcessValue(total_rev);
svenkugi 0:b6deec3905f4 434 dutyout = dist_pid.compute();
svenkugi 0:b6deec3905f4 435 }
svenkugi 0:b6deec3905f4 436 else{
svenkugi 0:b6deec3905f4 437 velocity_pid.setProcessValue(test_vel);
svenkugi 0:b6deec3905f4 438 dutyout = velocity_pid.compute();
svenkugi 0:b6deec3905f4 439 }
svenkugi 0:b6deec3905f4 440
svenkugi 0:b6deec3905f4 441 //dist_pid.setProcessValue(total_rev);
svenkugi 0:b6deec3905f4 442 //dutyout = dist_pid.compute();
svenkugi 0:b6deec3905f4 443
svenkugi 0:b6deec3905f4 444 //tmp.stop();
svenkugi 0:b6deec3905f4 445 //test_time = tmp.read();
svenkugi 0:b6deec3905f4 446 //tmp.reset();
svenkugi 0:b6deec3905f4 447 }
svenkugi 0:b6deec3905f4 448
svenkugi 0:b6deec3905f4 449 /*_______________________Testing and Tuning Function__________________________*/
svenkugi 0:b6deec3905f4 450
svenkugi 0:b6deec3905f4 451 /*Measures Angular Velocity using PhotoInterrupters by checking time taken to go
svenkugi 0:b6deec3905f4 452 from State 4 to State 4 in this case. Avoid sensor phasing as it measures one
svenkugi 0:b6deec3905f4 453 complete cycle */
svenkugi 0:b6deec3905f4 454
svenkugi 0:b6deec3905f4 455 void meas_velocity(){
svenkugi 0:b6deec3905f4 456
svenkugi 0:b6deec3905f4 457 intState = readRotorState();
svenkugi 0:b6deec3905f4 458 driveto = (intState-orState+direction*lead+6)%6;
svenkugi 0:b6deec3905f4 459 motorOut(driveto);
svenkugi 0:b6deec3905f4 460
svenkugi 0:b6deec3905f4 461 while (1) {
svenkugi 0:b6deec3905f4 462
svenkugi 0:b6deec3905f4 463 pc.printf("Rotations per second: %f \n\r", angular_vel);
svenkugi 0:b6deec3905f4 464
svenkugi 0:b6deec3905f4 465 }
svenkugi 0:b6deec3905f4 466 }
svenkugi 0:b6deec3905f4 467
svenkugi 0:b6deec3905f4 468 void PID_tuning(){
svenkugi 0:b6deec3905f4 469
svenkugi 0:b6deec3905f4 470 dutyout = 0.5;
svenkugi 0:b6deec3905f4 471
svenkugi 0:b6deec3905f4 472 intState = readRotorState();
svenkugi 0:b6deec3905f4 473 driveto = (intState-orState+lead+6)%6;
svenkugi 0:b6deec3905f4 474 motorOut(driveto);
svenkugi 0:b6deec3905f4 475
svenkugi 0:b6deec3905f4 476 while (1) {
svenkugi 0:b6deec3905f4 477
svenkugi 0:b6deec3905f4 478 // Testing Step Response by increasing D.C. from 0.5 to 0.7
svenkugi 0:b6deec3905f4 479 // Gradient is equal to Kc
svenkugi 0:b6deec3905f4 480
svenkugi 0:b6deec3905f4 481 if(count > 3000){
svenkugi 0:b6deec3905f4 482 dutyout = 0.7;
svenkugi 0:b6deec3905f4 483 }
svenkugi 0:b6deec3905f4 484
svenkugi 0:b6deec3905f4 485 pc.printf("Duty Cycle: %f ", dutyout);
svenkugi 0:b6deec3905f4 486 pc.printf("Rotations per second: %f ", angular_vel);
svenkugi 0:b6deec3905f4 487 pc.printf("Count: %d \n\r", count);
svenkugi 0:b6deec3905f4 488
svenkugi 0:b6deec3905f4 489 }
svenkugi 0:b6deec3905f4 490 }
svenkugi 0:b6deec3905f4 491
svenkugi 0:b6deec3905f4 492 /*__________________________Main Function_____________________________________*/
svenkugi 0:b6deec3905f4 493 int main(){
svenkugi 0:b6deec3905f4 494
svenkugi 0:b6deec3905f4 495 //Start of Program
svenkugi 0:b6deec3905f4 496 pc.printf("STARTING SKAFMO BRUSHLESS MOTOR PROJECT! \n\r");
svenkugi 0:b6deec3905f4 497 led1 = 1;
svenkugi 0:b6deec3905f4 498
svenkugi 0:b6deec3905f4 499 //Run the motor synchronisation: orState is subtracted from future rotor state inputs
svenkugi 0:b6deec3905f4 500 orState = motorHome();
svenkugi 0:b6deec3905f4 501 //pc.printf("Synchronization Complete: Rotor and Motor aligned with Offset: %x\n\r",orState);
svenkugi 0:b6deec3905f4 502
svenkugi 0:b6deec3905f4 503 //Interrupts (Optical Disk State Change): Drives to next state, Measures whole revolution count, Measures angular velocity over a whole revolution
svenkugi 0:b6deec3905f4 504 InterruptI1.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 505 InterruptI1.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 506 InterruptI2.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 507 InterruptI2.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 508 InterruptI3.rise(&changestate_isr);
svenkugi 0:b6deec3905f4 509 InterruptI3.fall(&changestate_isr);
svenkugi 0:b6deec3905f4 510
svenkugi 0:b6deec3905f4 511 //Interrupts (Incremental Encoder CHA Phase)
svenkugi 0:b6deec3905f4 512 InterruptCHA.rise(&pid_isr);
svenkugi 0:b6deec3905f4 513
svenkugi 3:7ee013b0976e 514 // Melody in a Thread
svenkugi 3:7ee013b0976e 515 // PID in Thread
svenkugi 0:b6deec3905f4 516
svenkugi 4:5eb8ac894d0f 517 position_control(350.34, 2.0); // Second parameter is 1 or -1 (Backwards or Forwards Rotation)
svenkugi 3:7ee013b0976e 518
svenkugi 3:7ee013b0976e 519 }