Diff: motors.cpp

Revision:

8:6c92789d5f87

Parent:

6:b340a527add9

Child:

12:878c6e9d9e60

--- a/motors.cpp	Sun Oct 16 11:11:21 2016 +0000
+++ b/motors.cpp	Sun Oct 16 12:54:33 2016 +0000
@@ -1,11 +1,11 @@
 /* University of York Robotics Laboratory PsiSwarm Library: Motor Functions Source File
- * 
+ *
  * Copyright 2016 University of York
  *
- * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. 
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0
  * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and limitations under the License.
  *
  * File: motors.cpp
@@ -13,7 +13,7 @@
  * (C) Dept. Electronics & Computer Science, University of York
  * James Hilder, Alan Millard, Alexander Horsfield, Homero Elizondo, Jon Timmis
  *
- * PsiSwarm Library Version: 0.7
+ * PsiSwarm Library Version: 0.8
  *
  * October 2016
  *
@@ -25,35 +25,35 @@
 Timeout time_based_action_timeout;
 char brake_when_done = 0;
 
-void set_left_motor_speed(float speed)
+void Motors::set_left_motor_speed(float speed)
 {
     motor_left_speed = speed;
     motor_left_brake = 0;
     IF_update_motors();
 }
 
-void set_right_motor_speed(float speed)
+void Motors::set_right_motor_speed(float speed)
 {
     motor_right_speed = speed;
     motor_right_brake = 0;
     IF_update_motors();
 }
 
-void brake_left_motor()
+void Motors::brake_left_motor()
 {
     motor_left_speed = 0;
     motor_left_brake = 1;
     IF_update_motors();
 }
 
-void brake_right_motor()
+void Motors::brake_right_motor()
 {
     motor_right_speed = 0;
     motor_right_brake = 1;
     IF_update_motors();
 }
 
-void brake()
+void Motors::brake()
 {
     motor_left_speed = 0;
     motor_right_speed = 0;
@@ -62,7 +62,7 @@
     IF_update_motors();
 }
 
-void stop()
+void Motors::stop()
 {
     motor_left_speed = 0;
     motor_right_speed = 0;
@@ -71,7 +71,7 @@
     IF_update_motors();
 }
 
-void forward(float speed)
+void Motors::forward(float speed)
 {
     motor_left_speed = speed;
     motor_right_speed = speed;
@@ -80,7 +80,7 @@
     IF_update_motors();
 }
 
-void backward(float speed)
+void Motors::backward(float speed)
 {
     motor_left_speed = -speed;
     motor_right_speed = -speed;
@@ -89,7 +89,7 @@
     IF_update_motors();
 }
 
-void turn(float speed)
+void Motors::turn(float speed)
 {
     //A positive turn is clockwise
     motor_left_speed = speed;
@@ -100,47 +100,48 @@
 }
 
 //Forward for a set period of time
-void time_based_forward(float speed, int microseconds, char brake)
+void Motors::time_based_forward(float speed, int microseconds, char brake)
 {
     //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
     IF_check_time_for_existing_time_based_action();
-        
+
     //Start moving
     forward(speed);
     brake_when_done = brake;
-    time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
+    time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,microseconds);
 }
 
 //Turn for a set period of time
-void time_based_turn(float speed, int microseconds, char brake)
+void Motors::time_based_turn(float speed, int microseconds, char brake)
 {
     //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
     IF_check_time_for_existing_time_based_action();
-        
+
     //Start turning
     turn(speed);
     brake_when_done = brake;
-    time_based_action_timeout.attach_us(&IF_end_time_based_action,microseconds);
+    time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,microseconds);
 }
 
 //Returns the limit of degrees available to turn in given time
-float get_maximum_turn_angle(int microseconds){
+float Motors::get_maximum_turn_angle(int microseconds)
+{
     //We can turn at about 270 degrees per second at full speed
     return (microseconds * 0.00027);
 }
 
 //Return the time in microseconds that performing the turn will take
-int get_time_based_turn_time(float speed, float degrees)
+int Motors::get_time_based_turn_time(float speed, float degrees)
 {
     //Check sign of degrees
     if(degrees < 0) degrees =- degrees;
-     
+
     //Main calculation for turn time
     float turn_time = degrees / ((290 * speed));
 
     //Add a hard offset of 4ms to account for start\stop time
     if(degrees > 4) {
-       turn_time += 0.004;
+        turn_time += 0.004;
     } else turn_time += 0.002;
 
     // Add offset for slow speed
@@ -157,15 +158,15 @@
         float short_offset_multiplier = 1.0 + (0.9 / degrees);
         turn_time *= short_offset_multiplier;
     }
-    
-    // Convert to uS 
+
+    // Convert to uS
     turn_time *= 1000000;
-    
-    return (int) turn_time;   
+
+    return (int) turn_time;
 }
 
 //Turn the robot a set number of degrees [using time estimation to end turn]
-int time_based_turn_degrees(float speed, float degrees, char brake)
+int Motors::time_based_turn_degrees(float speed, float degrees, char brake)
 {
     if(speed < 0 || speed > 1 || degrees == 0) {
         debug("Invalid values to time based turn: speed=%f degrees=$f\n",speed,degrees);
@@ -173,43 +174,42 @@
     } else {
         //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
         IF_check_time_for_existing_time_based_action();
-        
+
         //Calculate turn time using get_time_based_turn_time
         int turn_time = get_time_based_turn_time(speed,degrees);
-        
+
         //Set correct turn direction (-degrees is a counter-clockwise turn)
-       if(degrees < 0) {
+        if(degrees < 0) {
             degrees=-degrees;
             speed=-speed;
         }
 
         //Start turning
         turn(speed);
-        
+
         brake_when_done = brake;
-        time_based_action_timeout.attach_us(&IF_end_time_based_action,turn_time);
+        time_based_action_timeout.attach_us(this,&Motors::IF_end_time_based_action,turn_time);
         return turn_time;
     }
 }
 
 //Check if a current time based action is running - if it is, throw a warning and cancel its timeout
-void IF_check_time_for_existing_time_based_action()
+void Motors::IF_check_time_for_existing_time_based_action()
 {
-    if(time_based_motor_action == 1){
-        time_based_action_timeout.detach(); 
+    if(time_based_motor_action == 1) {
+        time_based_action_timeout.detach();
         debug("WARNING: New time-based action called before previous action finished!\n");
-    }   
-    else time_based_motor_action = 1;
+    } else time_based_motor_action = 1;
 }
 
-void IF_end_time_based_action()
+void Motors::IF_end_time_based_action()
 {
     if(brake_when_done == 1)brake();
     else stop();
     time_based_motor_action = 0;
 }
 
-void IF_update_motors()
+void Motors::IF_update_motors()
 {
     if(motor_left_speed > 1.0) {
         motor_left_speed = 1.0;
@@ -264,27 +264,27 @@
 }
 
 
-float IF_calibrated_left_speed(float speed)
+float Motors::IF_calibrated_left_speed(float speed)
 {
     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
     float adjusted  = IF_calibrated_speed(speed);
-    if(use_motor_calibration){
+    if(use_motor_calibration) {
         adjusted *= left_motor_calibration_value;
     }
     return adjusted;
 }
 
-float IF_calibrated_right_speed(float speed)
+float Motors::IF_calibrated_right_speed(float speed)
 {
     //Takes the input value, adds an offset if OFFSET_MOTORS enabled and weights the value if USE_MOTOR_CALIBRATION enabled
     float adjusted  = IF_calibrated_speed(speed);
-    if(use_motor_calibration){
+    if(use_motor_calibration) {
         adjusted *= right_motor_calibration_value;
     }
     return adjusted;
 }
 
-float IF_calibrated_speed(float speed)
+float Motors::IF_calibrated_speed(float speed)
 {
     if(speed == 0) return 0;
     //Converts an input value to take account of the stall speed of the motor; aims to produce a more linear speed
@@ -296,7 +296,7 @@
     return adjusted;
 }
 
-void IF_init_motors()
+void Motors::init_motors()
 {
     // Motor PWM outputs work optimally at 25kHz frequency
     motor_left_f.period_us(40);