Austin Mitchell / Mbed 2 deprecated RobotFightingControl

Sword fighting robots WIP

Dependencies:   mbed PinDetect

main.cpp@0:e8eecd4b9a3d, 2018-12-06 (annotated)

Committer:
amitchell41
Date:
Thu Dec 06 01:30:51 2018 +0000
Revision:
0:e8eecd4b9a3d
Swing batta batta swing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amitchell41 0:e8eecd4b9a3d 1 #include "meArm.h"
amitchell41 0:e8eecd4b9a3d 2 #include "Servo.h"
amitchell41 0:e8eecd4b9a3d 3 #include "MMA8452.h"
amitchell41 0:e8eecd4b9a3d 4 #include "mbed.h"
amitchell41 0:e8eecd4b9a3d 5 #include "PinDetect.h"
amitchell41 0:e8eecd4b9a3d 6
amitchell41 0:e8eecd4b9a3d 7 meArm arm;
amitchell41 0:e8eecd4b9a3d 8
amitchell41 0:e8eecd4b9a3d 9 Serial pc(USBTX,USBRX);
amitchell41 0:e8eecd4b9a3d 10
amitchell41 0:e8eecd4b9a3d 11 DigitalOut myLed(LED1);
amitchell41 0:e8eecd4b9a3d 12 PinDetect pb1(p20);
amitchell41 0:e8eecd4b9a3d 13
amitchell41 0:e8eecd4b9a3d 14 MMA8452 mma(p28, p27, 100000);
amitchell41 0:e8eecd4b9a3d 15 int x, y, z;
amitchell41 0:e8eecd4b9a3d 16
amitchell41 0:e8eecd4b9a3d 17 //Change these values if accelerometer reading are different:
amitchell41 0:e8eecd4b9a3d 18 //How far the accerometer is tilted before starting to move the arm:
amitchell41 0:e8eecd4b9a3d 19 const int MovementThreshold = 18;
amitchell41 0:e8eecd4b9a3d 20
amitchell41 0:e8eecd4b9a3d 21 //The average zero acceleration values read from the accelerometer for each axis:
amitchell41 0:e8eecd4b9a3d 22 const int ZeroXValue = 0;
amitchell41 0:e8eecd4b9a3d 23 const int ZeroYValue = 0;
amitchell41 0:e8eecd4b9a3d 24 const int ZeroZValue = 0;
amitchell41 0:e8eecd4b9a3d 25
amitchell41 0:e8eecd4b9a3d 26 //The maximum (positive) acceleration values read from the accelerometer for each axis:
amitchell41 0:e8eecd4b9a3d 27 const int MaxXValue = 4096;
amitchell41 0:e8eecd4b9a3d 28 const int MaxYValue = 4096;
amitchell41 0:e8eecd4b9a3d 29 const int MaxZValue = 4096;
amitchell41 0:e8eecd4b9a3d 30
amitchell41 0:e8eecd4b9a3d 31 //The minimum (negative) acceleration values read from the accelerometer for each axis:
amitchell41 0:e8eecd4b9a3d 32 const int MinXValue = -4096;
amitchell41 0:e8eecd4b9a3d 33 const int MinYValue = -4096;
amitchell41 0:e8eecd4b9a3d 34 const int MinZValue = -4096;
amitchell41 0:e8eecd4b9a3d 35
amitchell41 0:e8eecd4b9a3d 36 //The sign of the arm movement relative to the acceleration.
amitchell41 0:e8eecd4b9a3d 37 //If arm is going in the opposite direction you think it should go, change the sign for the appropriate axis.
amitchell41 0:e8eecd4b9a3d 38 const int XSign = 1;
amitchell41 0:e8eecd4b9a3d 39 const int YSign = 1;
amitchell41 0:e8eecd4b9a3d 40 const int ZSign = 1;
amitchell41 0:e8eecd4b9a3d 41
amitchell41 0:e8eecd4b9a3d 42 //The maximum speed in each axis (x and y)
amitchell41 0:e8eecd4b9a3d 43 //that the arm should move. Set this to a higher or lower number if the arm does not move fast enough or is too fast.
amitchell41 0:e8eecd4b9a3d 44 const int MaxArmMovement = 50;
amitchell41 0:e8eecd4b9a3d 45
amitchell41 0:e8eecd4b9a3d 46 //This reduces the 'twitchiness' of the cursor by calling a delay function at the end of the main loop.
amitchell41 0:e8eecd4b9a3d 47 //There are better way to do this without delaying the whole microcontroller, but that is left for another tutorial or project.
amitchell41 0:e8eecd4b9a3d 48 const int ArmDelay = .001;
amitchell41 0:e8eecd4b9a3d 49
amitchell41 0:e8eecd4b9a3d 50 //Function to process the acclerometer data
amitchell41 0:e8eecd4b9a3d 51 //and send mouse movement information via USB
amitchell41 0:e8eecd4b9a3d 52 void processAccelerometer(int16_t XReading, int16_t YReading, int16_t ZReading)
amitchell41 0:e8eecd4b9a3d 53 {
amitchell41 0:e8eecd4b9a3d 54 //Initialize values for the mouse cursor movement.
amitchell41 0:e8eecd4b9a3d 55 int16_t ArmXMovement = 0;
amitchell41 0:e8eecd4b9a3d 56 int16_t ArmYMovement = 0;
amitchell41 0:e8eecd4b9a3d 57 int16_t ArmZMovement = 0;
amitchell41 0:e8eecd4b9a3d 58
amitchell41 0:e8eecd4b9a3d 59 //Calculate mouse movement
amitchell41 0:e8eecd4b9a3d 60 //If the analog X reading is ouside of the zero threshold...
amitchell41 0:e8eecd4b9a3d 61 if( MovementThreshold < abs( XReading - ZeroXValue ) ){
amitchell41 0:e8eecd4b9a3d 62 //...calculate X mouse movement based on how far the X acceleration is from its zero value.
amitchell41 0:e8eecd4b9a3d 63 ArmXMovement = XSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxXValue - MinXValue ) ) * ( XReading - MinXValue ) ) - MaxArmMovement );
amitchell41 0:e8eecd4b9a3d 64 //it could use some improvement, like making it trigonometric.
amitchell41 0:e8eecd4b9a3d 65 } else {
amitchell41 0:e8eecd4b9a3d 66 //Within the zero threshold, the cursor does not move in the X.
amitchell41 0:e8eecd4b9a3d 67 ArmXMovement = 0;
amitchell41 0:e8eecd4b9a3d 68 }
amitchell41 0:e8eecd4b9a3d 69
amitchell41 0:e8eecd4b9a3d 70 //If the analog Y reading is ouside of the zero threshold...
amitchell41 0:e8eecd4b9a3d 71 if( MovementThreshold < abs( YReading - ZeroYValue ) ){
amitchell41 0:e8eecd4b9a3d 72 //...calculate Y mouse movement based on how far the Y acceleration is from its zero value.
amitchell41 0:e8eecd4b9a3d 73 ArmYMovement = YSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxYValue - MinYValue ) ) * ( YReading - MinYValue ) ) - MaxArmMovement );
amitchell41 0:e8eecd4b9a3d 74 //it could use some improvement, like making it trigonometric.
amitchell41 0:e8eecd4b9a3d 75 } else {
amitchell41 0:e8eecd4b9a3d 76 //Within the zero threshold, the cursor does not move in the Y.
amitchell41 0:e8eecd4b9a3d 77 ArmYMovement = 0;
amitchell41 0:e8eecd4b9a3d 78 }
amitchell41 0:e8eecd4b9a3d 79
amitchell41 0:e8eecd4b9a3d 80 //Calculate mouse movement
amitchell41 0:e8eecd4b9a3d 81 //If the analog Z reading is ouside of the zero threshold...
amitchell41 0:e8eecd4b9a3d 82 if( MovementThreshold < abs( ZReading - ZeroZValue ) )
amitchell41 0:e8eecd4b9a3d 83 {
amitchell41 0:e8eecd4b9a3d 84 //...calculate Z mouse movement based on how far the Z acceleration is from its zero value.
amitchell41 0:e8eecd4b9a3d 85 ArmZMovement = ZSign * ( ( ( (float)( 2 * MaxArmMovement ) / ( MaxZValue - MinZValue ) ) * ( ZReading - MinZValue ) ) - MaxArmMovement );
amitchell41 0:e8eecd4b9a3d 86 //it could use some improvement, like making it trigonometric.
amitchell41 0:e8eecd4b9a3d 87 }
amitchell41 0:e8eecd4b9a3d 88 else
amitchell41 0:e8eecd4b9a3d 89 {
amitchell41 0:e8eecd4b9a3d 90 //Within the zero threshold, the cursor does not move in the X.
amitchell41 0:e8eecd4b9a3d 91 ArmZMovement = 0;
amitchell41 0:e8eecd4b9a3d 92 }
amitchell41 0:e8eecd4b9a3d 93 arm.gotoPoint(ArmXMovement, ArmYMovement, ArmZMovement); // otherwise just move mouse
amitchell41 0:e8eecd4b9a3d 94 }
amitchell41 0:e8eecd4b9a3d 95
amitchell41 0:e8eecd4b9a3d 96 void swing(){
amitchell41 0:e8eecd4b9a3d 97 arm.openGripper();
amitchell41 0:e8eecd4b9a3d 98 arm.closeGripper();
amitchell41 0:e8eecd4b9a3d 99 }
amitchell41 0:e8eecd4b9a3d 100
amitchell41 0:e8eecd4b9a3d 101 int main() {
amitchell41 0:e8eecd4b9a3d 102 pb1.mode(PullUp);
amitchell41 0:e8eecd4b9a3d 103 mma.readXYZCounts(&x, &y, &z); // get an initial read
amitchell41 0:e8eecd4b9a3d 104 arm.begin(); // Initialize arm
amitchell41 0:e8eecd4b9a3d 105
amitchell41 0:e8eecd4b9a3d 106 while(1) {
amitchell41 0:e8eecd4b9a3d 107
amitchell41 0:e8eecd4b9a3d 108 if(!pb1){
amitchell41 0:e8eecd4b9a3d 109 myLed=1;
amitchell41 0:e8eecd4b9a3d 110 swing();
amitchell41 0:e8eecd4b9a3d 111 myLed=0;
amitchell41 0:e8eecd4b9a3d 112 }
amitchell41 0:e8eecd4b9a3d 113
amitchell41 0:e8eecd4b9a3d 114 mma.readXYZCounts(&x, &y, &z); // // Read the 'raw' data in 14-bit counts
amitchell41 0:e8eecd4b9a3d 115 printf("\n(%.2d,%.2d,%.2d)", x,y,z);
amitchell41 0:e8eecd4b9a3d 116 processAccelerometer(x,y,z); // Work with the read data
amitchell41 0:e8eecd4b9a3d 117 wait(ArmDelay); // wait until next reading - was 500 in Adafruit example
amitchell41 0:e8eecd4b9a3d 118 }
amitchell41 0:e8eecd4b9a3d 119 }