fork of what I have been writing
Revision 12:38afe92e67d0, committed 2020-03-09
- Comitter:
- le1917
- Date:
- Mon Mar 09 15:02:47 2020 +0000
- Parent:
- 11:038d3ba0d720
- Child:
- 13:f6e37c21d31d
- Commit message:
- Merger Motor with Noe - seems to work;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ES_CW2_Starter_STARFISH/MotorControl.h Mon Mar 09 15:02:47 2020 +0000 @@ -0,0 +1,293 @@ +//Photointerrupter input pins +#define I1pin D3 +#define I2pin D6 +#define I3pin D5 + +//Incremental encoder input pins +#define CHApin D12 +#define CHBpin D11 + +//Motor Drive output pins //Mask in output byte +#define L1Lpin D1 //0x01 +#define L1Hpin A3 //0x02 +#define L2Lpin D0 //0x04 +#define L2Hpin A6 //0x08 +#define L3Lpin D10 //0x10 +#define L3Hpin D2 //0x20 + +#define PWMpin D9 + +//Motor current sense +#define MCSPpin A1 +#define MCSNpin A0 + +//Test outputs +#define TP0pin D4 +#define TP1pin D13 +#define TP2pin A2 + +//Mapping from sequential drive states to motor phase outputs +/* +State L1 L2 L3 +0 H - L +1 - H L +2 L H - +3 L - H +4 - L H +5 H L - +6 - - - +7 - - - +*/ +//Drive state to output table +const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; + +//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid +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 + +// MOTOR PARAMETERS // +const float kp_vel = 0.05; +//const float ki_vel = 0.03; +//const float kd_vel = 0.03; + +const float ki_vel = 0; +const float kd_vel = 0; + +const float kp_pos = 0.05; +const float kd_pos = 0; +const float pwm_period =0.25f; + +int target_velocity = 30; +float target_position = 500; + +// GLOBAL VARIABLES MOTOR // +int position = 0; +bool direction = 1; +int lead = 2; + +volatile int8_t orState = 0; //Rotot offset at motor state 0 +volatile int8_t intState = 0; +volatile int8_t intStateOld = 0; + +//Status LED +DigitalOut led1(LED1); + +//Photointerrupter inputs +InterruptIn I1(I1pin); +InterruptIn I2(I2pin); +InterruptIn I3(I3pin); + +//Motor Drive outputs +DigitalOut L1L(L1Lpin); +DigitalOut L1H(L1Hpin); +DigitalOut L2L(L2Lpin); +DigitalOut L2H(L2Hpin); +DigitalOut L3L(L3Lpin); +DigitalOut L3H(L3Hpin); + +DigitalOut TP1(TP1pin); +PwmOut MotorPWM(PWMpin); + +Ticker motorCtrlTicker; +Thread thread_motorCtrl (osPriorityNormal,1024); + +// DEBUG // +float velocity_global = 0; + +//Set a given drive state +void motorOut(int8_t driveState) +{ + + //Lookup the output byte from the drive state. + int8_t driveOut = driveTable[driveState & 0x07]; + + //Turn off first + if (~driveOut & 0x01) L1L = 0; + if (~driveOut & 0x02) L1H = 1; + if (~driveOut & 0x04) L2L = 0; + if (~driveOut & 0x08) L2H = 1; + if (~driveOut & 0x10) L3L = 0; + if (~driveOut & 0x20) L3H = 1; + + //Then turn on + if (driveOut & 0x01) L1L = 1; + if (driveOut & 0x02) L1H = 0; + if (driveOut & 0x04) L2L = 1; + if (driveOut & 0x08) L2H = 0; + if (driveOut & 0x10) L3L = 1; + if (driveOut & 0x20) L3H = 0; +} + +//Convert photointerrupter inputs to a rotor state +inline int8_t readRotorState() +{ + return stateMap[I1 + 2*I2 + 4*I3]; +} + + +void move() +{ + intState = readRotorState(); + + // Updates direction only if statechanges by 1 + // If state chance is missed then interrupt is unchanged + if( intState == 0 && intStateOld == 5) { + direction = 1; + position++; + } else if( intState == 5 && intStateOld == 0) { + direction = 0; + position--; + } else if ( intState == intStateOld + 1 ) { + direction = 1; + position++; + } else if (intState == intStateOld - 1) { + direction = 0; + position--; + } + + + intStateOld = intState; + + motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive +} + + +void motorCtrlTick() +{ + thread_motorCtrl.signal_set(0x1); + //osSignalSet(thread_motorCtrl, 0x1); +} + +float get_y_pos(float e, float change_e) +{ + return kp_pos*e + kd_pos*change_e; +} + +float get_motor_out(float velocity_out) +{ + float motor_out; + if( velocity_out < 0) { + motor_out = velocity_out*-1; + // motor_out = 0; + } else { + motor_out = velocity_out; + } + if (velocity_out > 1 || velocity_out < -1) { + motor_out = 1; + } + return motor_out; +} + +int get_current_position() +{ + return position; +} + +void attach_ISR() +{ + I1.rise(&move); + I1.fall(&move); + I2.rise(&move); + I2.fall(&move); + I3.rise(&move); + I3.fall(&move); +} + + +float combine_speed(float y_pos, float velocity_out, float velocity_error) +{ + if(velocity_error >= 0) { + if(y_pos < velocity_out) { + return y_pos; + } else { + return velocity_out; + } + } else { + if(y_pos > velocity_out) { + return y_pos; + } else { + return velocity_out; + } + } + return y_pos; +} + + +void update_lead(float velocity_out) +{ + // No functionality for breaking + if(velocity_out >= 0) { + lead = 2; + } else { + lead = -2; + } +} + +void motorCtrlFn() +{ + // motor controller variables + float current_position = 0; + int previous_position = 0; + float velocity = 0; + Timer timer_velocity; + uint32_t last_time_MtrCtlr; + float pos_error_old; + int i = 0; + + float velocity_integral=0; + float velocity_derivative=0; + float last_velocity_error = 0; + float velocity_error = 0; + float velocity_out = 0; + float velocity_factor = 1; + + motorCtrlTicker.attach_us(&motorCtrlTick,100000); + pos_error_old = target_position; + last_time_MtrCtlr = 0; + + MotorPWM.write(1); + MotorPWM.period(pwm_period); + + motorOut(0); + wait(3.0); + orState = readRotorState(); + + if(target_velocity > 0) { + lead = 2; + motorOut(1); + } else { + lead = -2; + motorOut(5); + } + + attach_ISR(); + + current_position = get_current_position(); + previous_position = current_position; + timer_velocity.start(); + + while(1) { + thread_motorCtrl.signal_wait(0x1); + + current_position = get_current_position(); + velocity_factor = (1000/(timer_velocity.read_ms()-last_time_MtrCtlr)); + velocity = ((current_position - previous_position)/6)*velocity_factor; + last_time_MtrCtlr = timer_velocity.read_ms(); + previous_position = current_position; + + velocity_error = target_velocity - velocity; + velocity_integral += velocity_error; + velocity_derivative = velocity_error - velocity_error; + velocity_out = (kp_vel*velocity_error) + (ki_vel*velocity_integral) + (kd_vel*velocity_derivative); + last_velocity_error = velocity_error; + + velocity_global = velocity; + + float motor_out = get_motor_out(velocity_out); + + update_lead(velocity_out); + MotorPWM.period(pwm_period); + MotorPWM.write(motor_out); + + } +} \ No newline at end of file
--- a/ES_CW2_Starter_STARFISH/import.h Fri Mar 06 14:27:16 2020 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,8 +0,0 @@ -#include "mbed.h" -#include "SHA256.h" -#include "rtos.h" -#include <stdlib.h> -#include <string> -#include "mutexes.h" -#include "SerialCommunication.h" -#include "CryptoMining.h"
--- a/ES_CW2_Starter_STARFISH/main.cpp Fri Mar 06 14:27:16 2020 +0000 +++ b/ES_CW2_Starter_STARFISH/main.cpp Mon Mar 09 15:02:47 2020 +0000 @@ -1,170 +1,28 @@ - -#include "import.h" +#include "mbed.h" +#include "SHA256.h" +#include "rtos.h" +#include <stdlib.h> +#include <string> +#include "mutexes.h" +#include "SerialCommunication.h" +#include "CryptoMining.h" +#include "MotorControl.h" // Declaration of threads Thread thread_crypto; Thread thread_processor; - // Timing variables for printing calculation rate Timer timer_nonce; uint32_t previous_time; -//Photointerrupter input pins -#define I1pin D3 -#define I2pin D6 -#define I3pin D5 - -//Incremental encoder input pins -#define CHApin D12 -#define CHBpin D11 - -//Motor Drive output pins //Mask in output byte -#define L1Lpin D1 //0x01 -#define L1Hpin A3 //0x02 -#define L2Lpin D0 //0x04 -#define L2Hpin A6 //0x08 -#define L3Lpin D10 //0x10 -#define L3Hpin D2 //0x20 - -#define PWMpin D9 - -//Motor current sense -#define MCSPpin A1 -#define MCSNpin A0 - -//Test outputs -#define TP0pin D4 -#define TP1pin D13 -#define TP2pin A2 - -//Mapping from sequential drive states to motor phase outputs -/* -State L1 L2 L3 -0 H - L -1 - H L -2 L H - -3 L - H -4 - L H -5 H L - -6 - - - -7 - - - -*/ -//Drive state to output table -const int8_t driveTable[] = {0x12, 0x18, 0x09, 0x21, 0x24, 0x06, 0x00, 0x00}; - -//Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid -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 - -//Phase lead to make motor spin -const int8_t lead = 2; //2 for forwards, -2 for backwards - -//Status LED -DigitalOut led1(LED1); - -//Photointerrupter inputs -InterruptIn I1(I1pin); -InterruptIn I2(I2pin); -InterruptIn I3(I3pin); - -//Motor Drive outputs -DigitalOut L1L(L1Lpin); -DigitalOut L1H(L1Hpin); -DigitalOut L2L(L2Lpin); -DigitalOut L2H(L2Hpin); -DigitalOut L3L(L3Lpin); -DigitalOut L3H(L3Hpin); - -DigitalOut TP1(TP1pin); -PwmOut MotorPWM(PWMpin); - -int8_t orState = 0; //Rotot offset at motor state 0 -int8_t intState = 0; -int8_t intStateOld = 0; - -//Set a given drive state -void motorOut(int8_t driveState) -{ - - //Lookup the output byte from the drive state. - int8_t driveOut = driveTable[driveState & 0x07]; - - //Turn off first - if (~driveOut & 0x01) - L1L = 0; - if (~driveOut & 0x02) - L1H = 1; - if (~driveOut & 0x04) - L2L = 0; - if (~driveOut & 0x08) - L2H = 1; - if (~driveOut & 0x10) - L3L = 0; - if (~driveOut & 0x20) - L3H = 1; - - //Then turn on - if (driveOut & 0x01) - L1L = 1; - if (driveOut & 0x02) - L1H = 0; - if (driveOut & 0x04) - L2L = 1; - if (driveOut & 0x08) - L2H = 0; - if (driveOut & 0x10) - L3L = 1; - if (driveOut & 0x20) - L3H = 0; -} - -//Convert photointerrupter inputs to a rotor state -inline int8_t readRotorState() -{ - return stateMap[I1 + 2 * I2 + 4 * I3]; -} - -//Basic synchronisation routine -int8_t motorHome() -{ - //Put the motor in drive state 0 and wait for it to stabilise - motorOut(0); - wait(2.0); - - //Get the rotor state - return readRotorState(); -} - -void move() -{ - intState = readRotorState(); - motorOut((intState - orState + lead + 6) % 6); //+6 to make sure the remainder is positive - intStateOld = intState; -} - - //Main int main() { pc.attach(&serialISR); - const int32_t PWM_PRD = 2500; - MotorPWM.period_us(PWM_PRD); - MotorPWM.pulsewidth_us(PWM_PRD); - pc.printf("Hello\n\r"); - - //Run the motor synchronisation - orState = motorHome(); - pc.printf("Rotor origin: %x\n\r", orState); - - I1.rise(&move); - I1.fall(&move); - I2.rise(&move); - I2.fall(&move); - I3.rise(&move); - I3.fall(&move); + thread_motorCtrl.start(motorCtrlFn); // Initialize threads and timers timer_nonce.start(); @@ -172,26 +30,23 @@ thread_processor.start(thread_processor_callback); uint8_t hash[32]; - while (1) - { + while (1) { // Set main as lowest priority thread NewKey_mutex.lock(); - *key = NewKey; + *key = NewKey; NewKey_mutex.unlock(); SHA256::computeHash(hash, (uint8_t *)sequence, 64); *nonce = *nonce + 1; - if ((hash[0] == 0) && (hash[1] == 0)) - { + if ((hash[0] == 0) && (hash[1] == 0)) { last_nonce_number = successful_nonce; putMessageCrypto(*nonce); successful_nonce++; } - if ((timer_nonce.read_ms() - previous_time) > 1000) - { + if ((timer_nonce.read_ms() - previous_time) > 1000) { //pc.printf("Computation Rate: %lu computation /sec\n\r", (*nonce - last_nonce_number)); last_nonce_number = *nonce; previous_time = timer_nonce.read_ms(); @@ -199,4 +54,4 @@ } return 0; -} +} \ No newline at end of file