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@10:e974ee1ea1f0, 2017-03-24 (annotated)

Committer:
svenkugi
Date:
Fri Mar 24 14:00:35 2017 +0000
Revision:
10:e974ee1ea1f0
Parent:
6:8d18cdcabc3a
Child:
11:f72be5748371
Child:
12:943207547cb1
ALEX

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