Luccas Eagles
added melody to old motorcontol
Diff: main.cpp
- Revision:
- 24:6d85dda245e1
- Parent:
- 23:e5a2e8cef243
- Child:
- 25:4b893287d750
--- a/main.cpp Fri Mar 13 20:26:17 2020 +0000
+++ b/main.cpp Fri Mar 13 20:31:22 2020 +0000
@@ -6,22 +6,22 @@
#define I3pin D5
//Incremental encoder input pins
-#define CHApin D12
-#define CHBpin D11
+#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 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
+#define MCSPpin A1
+#define MCSNpin A0
//Test outputs
#define TP0pin D4
@@ -30,8 +30,6 @@
Serial pc(SERIAL_TX, SERIAL_RX);
-
-
// Controller variables
Timer timer_velocity;
uint32_t last_time_MtrCtlr;
@@ -39,27 +37,26 @@
// Velocity Controller Variables
float target_velocity = 30;
-float integral_vel = 0;
-float derivative_vel = 0;
-float last_vel_error = 0;
+float integral_vel = 0;
+float derivative_vel = 0;
+float last_vel_error = 0;
int previous_position = 0;
-int current_position = 0;
+int current_position = 0;
float kp_vel = 20;
-float ki_vel = 0;
-float kd_vel = 0;
+float ki_vel = 0;
+float kd_vel = 0;
// Position Controller Variables
float target_position = 500;
-float integral_pos = 0;
-float derivative_pos = 0;
-float last_pos_error = 0;
+float integral_pos = 0;
+float derivative_pos = 0;
+float last_pos_error = 0;
float kp_pos = 20;
-float ki_pos = 0;
-float kd_pos = 10;
-
+float ki_pos = 0;
+float kd_pos = 10;
//Mapping from sequential drive states to motor phase outputs
/*
@@ -74,15 +71,13 @@
7 - - -
*/
//Drive state to output table
-const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
+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, 0x05, 0x03, 0x04, 0x01, 0x00, 0x02, 0x07};
//const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
-const float pwm_period =0.25f;
-
-
+const float pwm_period = 0.25f;
// SHARED GLOBAL VARIABLES //
Semaphore pos_semaphore(0);
@@ -114,13 +109,12 @@
PwmOut MotorPWM(PWMpin);
Ticker motorCtrlTicker;
-Thread thread_motorCtrl (osPriorityNormal,1024);
+Thread thread_motorCtrl(osPriorityNormal, 1024);
-volatile int8_t orState = 0; //Rotot offset at motor state 0
+volatile int8_t orState = 0; //Rotot offset at motor state 0
volatile int8_t intState = 0;
volatile int8_t intStateOld = 0;
-
//Set a given drive state
void motorOut(int8_t driveState)
{
@@ -129,45 +123,63 @@
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;
+ 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;
+ 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];
+ 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) {
+ if (intState == 0 && intStateOld == 5)
+ {
direction = 1;
position++;
- } else if( intState == 5 && intStateOld == 0) {
+ }
+ else if (intState == 5 && intStateOld == 0)
+ {
direction = 0;
position--;
- } else if ( intState == intStateOld + 1 ) {
+ }
+ else if (intState == intStateOld + 1)
+ {
direction = 1;
position++;
- } else if (intState == intStateOld - 1) {
+ }
+ else if (intState == intStateOld - 1)
+ {
direction = 0;
position--;
}
@@ -176,9 +188,8 @@
intStateOld = intState;
- motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive
- }
-
+ motorOut((intState - orState + lead + 6) % 6); //+6 to make sure the remainder is positive
+}
void motorCtrlTick()
{
@@ -187,42 +198,49 @@
}
float get_vel_control_out(float velocity_error)
-{
- integral_vel += velocity_error;
- derivative_vel = velocity_error - last_vel_error;
- float velocity_out = (kp_vel*velocity_error) + (ki_vel*integral_vel) + (kd_vel*derivative_vel);
- last_vel_error = velocity_error;
+{
+ integral_vel += velocity_error;
+ derivative_vel = velocity_error - last_vel_error;
+ float velocity_out = (kp_vel * velocity_error) + (ki_vel * integral_vel) + (kd_vel * derivative_vel);
+ last_vel_error = velocity_error;
return velocity_out;
}
-float get_pos_control_out(float position_error){
- integral_pos += position_error;
- derivative_pos = position_error - last_pos_error;
- float velocity_out = (kp_pos*position_error) + (ki_pos*integral_pos) + (kd_pos*derivative_pos);
- last_pos_error = position_error;
- return velocity_out;
+float get_pos_control_out(float position_error)
+{
+ integral_pos += position_error;
+ derivative_pos = position_error - last_pos_error;
+ float velocity_out = (kp_pos * position_error) + (ki_pos * integral_pos) + (kd_pos * derivative_pos);
+ last_pos_error = position_error;
+ return velocity_out;
}
float get_motor_out(float velocity_out)
{
float motor_out;
- if( velocity_out < 0) {
- motor_out = velocity_out*-1;
- } else {
+ if (velocity_out < 0)
+ {
+ motor_out = velocity_out * -1;
+ }
+ else
+ {
motor_out = velocity_out;
}
- if (velocity_out > 1 || velocity_out < -1) {
+ if (velocity_out > 1 || velocity_out < -1)
+ {
motor_out = 1;
}
return motor_out;
}
-int get_current_position(){
+int get_current_position()
+{
pos_semaphore.wait();
return position;
}
-void attach_ISR(){
+void attach_ISR()
+{
I1.rise(&move);
I1.fall(&move);
I2.rise(&move);
@@ -231,42 +249,47 @@
I3.fall(&move);
}
-
float combine_control_out(float position_control_out, float velocity_control_out, float current_velocity)
{
- float velocity_out = 0;
- if(current_velocity <= 0) {
- velocity_out = std::max(position_control_out, velocity_control_out); }
- else {
- velocity_out = std::min(position_control_out, velocity_control_out);
+ float velocity_out = 0;
+ if (current_velocity <= 0)
+ {
+ velocity_out = std::max(position_control_out, velocity_control_out);
+ }
+ else
+ {
+ velocity_out = std::min(position_control_out, velocity_control_out);
}
return velocity_out;
}
-float get_current_velocity(float current_position) {
- float velocity_factor = (1000/(timer_velocity.read_ms()-last_time_MtrCtlr));
- float velocity = ((current_position - previous_position)/6)*velocity_factor;
- last_time_MtrCtlr = timer_velocity.read_ms();
- previous_position = current_position;
- return velocity;
+float get_current_velocity(float current_position)
+{
+ float velocity_factor = (1000 / (timer_velocity.read_ms() - last_time_MtrCtlr));
+ float velocity = ((current_position - previous_position) / 6) * velocity_factor;
+ last_time_MtrCtlr = timer_velocity.read_ms();
+ previous_position = current_position;
+ return velocity;
}
-void update_lead(float velocity_out){
+void update_lead(float velocity_out)
+{
// No functionality for breaking
- if(velocity_out >= 0){
+ if (velocity_out >= 0)
+ {
lead = 2;
}
- else {
+ else
+ {
lead = -2;
}
-
}
void motorInitSequence()
{
- motorCtrlTicker.attach_us(&motorCtrlTick,100000);
+ motorCtrlTicker.attach_us(&motorCtrlTick, 100000);
last_pos_error = target_position;
last_time_MtrCtlr = 0;
-
+
MotorPWM.write(1);
MotorPWM.period(pwm_period);
@@ -275,18 +298,22 @@
orState = readRotorState();
attach_ISR();
- if(target_velocity > 0){
+ if (target_velocity > 0)
+ {
lead = 2;
- for (int i = 1; i<4; i++){
+ for (int i = 1; i < 4; i++)
+ {
motorOut(i);
- wait(0.2);
+ wait(0.2);
}
}
- else{
+ else
+ {
lead = -2;
- for (int i = 5; i > 2 ; i --){
+ for (int i = 5; i > 2; i--)
+ {
motorOut(i);
- wait(0.2);
+ wait(0.2);
}
}
current_position = get_current_position();
@@ -295,40 +322,39 @@
}
void motorCtrlFn()
-{
- while(1) {
+{
+ while (1)
+ {
thread_motorCtrl.signal_wait(0x1);
current_position = get_current_position();
- float current_velocity = get_current_velocity(current_position);
+ float current_velocity = get_current_velocity(current_position);
- float velocity_error = target_velocity - current_velocity;
- float velocity_control_out = get_vel_control_out(velocity_error);
-
- float position_error = target_position - (current_position/6);
+ float velocity_error = target_velocity - current_velocity;
+ float velocity_control_out = get_vel_control_out(velocity_error);
+
+ float position_error = target_position - (current_position / 6);
float position_control_out = get_pos_control_out(position_error);
- float velocity_out = combine_control_out(position_control_out, velocity_control_out, current_velocity);
- float motor_out = get_motor_out(position_control_out);
+ float velocity_out = combine_control_out(position_control_out, velocity_control_out, current_velocity);
+ float motor_out = get_motor_out(position_control_out);
update_lead(velocity_out);
MotorPWM.period(pwm_period);
- MotorPWM.write(motor_out);
+ MotorPWM.write(motor_out);
- if(i > 10) {
- pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", current_velocity, (position/6), motor_out, velocity_out, lead);
+ if (i > 10)
+ {
+ pc.printf("Velocity = %f, Position = %f, MotorOut = %f, y = %f, lead = %d\r\n", current_velocity, (position / 6), motor_out, velocity_out, lead);
i = 0;
}
i++;
- }
}
-
+}
//Main
int main()
{
motorInitSequence();
thread_motorCtrl.start(motorCtrlFn);
-
}
-