test

Fork of motor by H29Ateam

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers motor.cpp Source File

motor.cpp

00001 #include "motor.h"
00002 
00003 int motor::write(int spd)
00004 {
00005     if(!inited) return -1;
00006     char cmd[1];
00007     if (spd > 0) {
00008         if(spd >= 255)  spd = 255;
00009         spd = map(spd, 1, 255, cwMin + 1, cwMax);
00010         cmd[0] = spd / 2 + 0b10000000;
00011         direction = 1;
00012     } else if (spd < 0) {
00013         if(spd <= -255)  spd = -255;
00014         spd = map(spd, -1, -255, ccwMin + 1, ccwMax);
00015         cmd[0] = spd / 2;
00016         direction = 0;
00017     } else {
00018         cmd[0] = H_B << 7;
00019     }
00020     speed = cmd[0] << 1;
00021     return i2c->write(number, cmd, 1);
00022 }
00023 
00024 uint8_t motor::make(int spd)
00025 {
00026     char cmd[1];
00027     if (spd > 0) {
00028         if(spd >= 255)  spd = 255;
00029         spd = map(spd, 1, 255, cwMin + 1, cwMax);
00030         cmd[0] = spd / 2 + 0b10000000;
00031     } else if (spd < 0) {
00032         if(spd <= -255)  spd = -255;
00033         spd = map(spd, -1, -255, ccwMin + 1, ccwMax);
00034         cmd[0] = spd / 2;
00035     } else {
00036         cmd[0] = 0;
00037     }
00038     return cmd[0];
00039 }
00040 
00041 uint8_t motor::init(uint8_t t, I2C *i)
00042 {
00043     char cmd[2] = {t>>1, t>>1};
00044     char data[1];
00045     
00046     inited = true;
00047     
00048     number = t;
00049     i2c = i;
00050     speed = 0;
00051     direction = 1;
00052     cwMax = 255;
00053     ccwMax = 255;
00054     cwMin = 0;
00055     ccwMin = 0;
00056     
00057     while(1) {
00058         i2c->write(t, cmd, 2);
00059         i2c->read(t | 1, data, 1);
00060         if(data[0] == (t >> 1)) break;
00061     }
00062     write(0);
00063     return 0;
00064 }
00065 
00066 long motor::map(long x, long in_min, long in_max, long out_min, long out_max)
00067 {
00068     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00069 }