Revision 29:78419e287e62, committed 2019-11-04

Comitter:

MatthewMaat

Date:

Mon Nov 04 21:20:11 2019 +0000

Parent:

28:5aece9593681

Commit message:

met comments

Changed in this revision

--- a/main.cpp	Mon Nov 04 11:42:06 2019 +0000
+++ b/main.cpp	Mon Nov 04 21:20:11 2019 +0000
@@ -6,6 +6,8 @@
 #include "BiQuad.h"
 #include "FastPWM.h"
 #include <iostream>
+
+//Inputs and outputs
 MODSERIAL pc(USBTX, USBRX);
 QEI motor2_pos (D8, D9, NC, 32);
 QEI motor1_pos (D12, D13, NC, 32);
@@ -35,6 +37,7 @@
 InterruptIn left_button(D3);
 InterruptIn right_button(D2);
 
+//variables
 volatile float P0;
 volatile float P1;
 volatile float EMG_min0=1;
@@ -93,7 +96,7 @@
 volatile float theta1_ref;
 volatile float theta2_ref;
 
-void read_emg()
+void read_emg()//read and filter EMG
 {
     //EMG signal 0
     static int count0=0;
@@ -142,7 +145,7 @@
     ledblue=1;
 }
 
-void get_angles(void)
+void get_angles(void) //calculate angles from encoder output
 {
     float pulses1=motor1_pos.getPulses();
     float pulses2=motor2_pos.getPulses();
@@ -150,7 +153,7 @@
     theta2=angle2_offset+pulses2*17.0/16.0*2*pi/131.0/32.0; 
 }
 
-void pos_cal(void)
+void pos_cal(void) // main function for calibrating the motors
 {
     float t=state_time.read();
     static int pos_time_counter=0;
@@ -199,7 +202,7 @@
     
 }
 
-void record_min_max(void)
+void record_min_max(void)// main function for calibrating EMG
 {
     float t=state_time.read();
     if(t>0.4)
@@ -223,6 +226,7 @@
     }
 }
 
+//timed functions for EMG control
 void unignore_peaksx(void)
 {
     ignore_peaksx=false;
@@ -240,7 +244,7 @@
     ignore_turny=true;
 }
 
-void set_v_refxy(void)
+void set_v_refxy(void)//set reference velocities based on EMG input
 {
    float Q0;
    Q0=2.0f*(P0-(EMG_min0+EMG_max0)/2.0f)/(EMG_max0-EMG_min0);
@@ -324,7 +328,7 @@
     }
 }
 
-void left_fake_emg(void)
+void left_fake_emg(void)// optional- set refence x direction based on pressing a button
 {
     if (!ignore_peaksx)
    {
@@ -357,7 +361,7 @@
     }
 }
 
-void right_fake_emg(void)
+void right_fake_emg(void)// optional- set refence y direction based on pressing a button
 {
     if (!ignore_peaksy)
    {
@@ -390,7 +394,7 @@
     }
 }
 
-void set_ref_fake_emg(void)
+void set_ref_fake_emg(void) // optional- set refence x/y velocity based on pressing a button
 {
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
@@ -412,7 +416,7 @@
     }
 }
 
-void error(void){
+void error(void){ //calculate the errors on the motor angles based on input reference velocities or positions
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
     float y_pos=tan(theta1)*x_pos;
     float r1=sqrt(pow(x_pos,2)+pow(y_pos,2));
@@ -493,7 +497,7 @@
     
     }
     
-float PID1(float err){
+float PID1(float err){//Calculate controller output for motor 1 based on error 1
     //P action
     u_p = Kp * err;
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
@@ -523,7 +527,7 @@
     return u;
     }
     
-float PID2(float err){
+float PID2(float err){//Calculate controller output for motor 2 based on error 2
     //P action
     u_p = Kp * err;
     float x_pos=0.35*tan(theta2)/(tan(theta1)+tan(theta2));
@@ -550,7 +554,7 @@
     u = r2/3.0f*(u_p + u_i+u_d);
     return u;
     }
-void set_refxy(void)
+void set_refxy(void)//set reference velocities in the demo mode
 {
     float t=state_time.read();
     int tfloor=floor(t)/1;
@@ -678,7 +682,7 @@
     //y_ref=asdfgh+0.125;
 }
  
-void setPWM_controller(void){
+void setPWM_controller(void){//set motor PWM values based on controller output
     error();
     u_1 = PID1(error_1);
     u_2 = PID2(error_2);
@@ -699,7 +703,7 @@
     
     }
 
-void sample()
+void sample()// main funtion loop
 {
     get_angles();
     scope.set(0,theta1);
@@ -770,12 +774,12 @@
     //Preventing the machine from breaking
 }
 
-void error_occur()
+void error_occur()//executed when error button is pressed
 {
     currentState=Failure;
 }
 
-void return_home()
+void return_home()//main function for homing state
 {
     theta1_ref=pi/4.0f;
     theta2_ref=pi/4.0f;